diff --git a/flight/AHRS/Makefile b/flight/AHRS/Makefile
index 17586c7f9..bbec1dcd7 100644
--- a/flight/AHRS/Makefile
+++ b/flight/AHRS/Makefile
@@ -94,6 +94,7 @@ SRC += $(PIOSSTM32F10X)/pios_delay.c
 SRC += $(PIOSSTM32F10X)/pios_usart.c
 SRC += $(PIOSSTM32F10X)/pios_irq.c
 SRC += $(PIOSSTM32F10X)/pios_adc.c
+#SRC += $(PIOSSTM32F10X)/pios_i2c.c
 #SRC += $(PIOSSTM32F10X)/pios_spi.c
 
 ## PIOS Hardware (Common)
diff --git a/flight/PiOS/STM32F10x/pios_usart.c b/flight/PiOS/STM32F10x/pios_usart.c
index 114f9b7a0..7d829d6b0 100644
--- a/flight/PiOS/STM32F10x/pios_usart.c
+++ b/flight/PiOS/STM32F10x/pios_usart.c
@@ -193,12 +193,17 @@ void PIOS_USART_ChangeBaud(USARTNumTypeDef usart, uint32_t baud)
 	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
 	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
 	USART_InitStructure.USART_BaudRate = baud;
-	if (usart == USART_1)
+	if (usart == USART_1) {
 		USART_Init(PIOS_USART1_USART, &USART_InitStructure);
-	else if (usart == USART_2)
+	} else if (usart == USART_2) {
+#if (PIOS_USART2_ENABLED)
 		USART_Init(PIOS_USART2_USART, &USART_InitStructure);
-	else if (usart == USART_3)
+#endif
+	} else if (usart == USART_3) {
+#if (PIOS_USART3_ENABLED)
 		USART_Init(PIOS_USART3_USART, &USART_InitStructure);
+#endif
+	}
 }
 
 /**