diff --git a/flight/PiOS/Common/pios_com.c b/flight/PiOS/Common/pios_com.c
index ee05c789a..2e2a64e4a 100644
--- a/flight/PiOS/Common/pios_com.c
+++ b/flight/PiOS/Common/pios_com.c
@@ -71,13 +71,13 @@ int32_t PIOS_COM_SendBufferNonBlocking(COMPortTypeDef port, uint8_t *buffer, uin
 {
 	/* Branch depending on selected port */
 	switch(port) {
-		case COM_DEBUG_UART:
+		case COM_DEBUG_USART:
 			return PIOS_USART_TxBufferPutMoreNonBlocking(COM_DEBUG_PORT, buffer, len);
-		case COM_GPS_UART:
+		case COM_GPS_USART:
 			return PIOS_USART_TxBufferPutMoreNonBlocking(GPS, buffer, len);
-		case COM_TELEM_UART:
+		case COM_TELEM_USART:
 			return PIOS_USART_TxBufferPutMoreNonBlocking(TELEM, buffer, len);
-		case COM_AUX_UART:
+		case COM_AUX_USART:
 			return PIOS_USART_TxBufferPutMoreNonBlocking(AUX, buffer, len);
 		case COM_USB_HID:
 			return PIOS_USB_HID_TxBufferPutMoreNonBlocking(buffer, len);
@@ -100,13 +100,13 @@ int32_t PIOS_COM_SendBuffer(COMPortTypeDef port, uint8_t *buffer, uint16_t len)
 {
 	/* Branch depending on selected port */
 	switch(port) {
-		case COM_DEBUG_UART:
+		case COM_DEBUG_USART:
 			return PIOS_USART_TxBufferPutMore(COM_DEBUG_PORT, buffer, len);
-		case COM_GPS_UART:
+		case COM_GPS_USART:
 			return PIOS_USART_TxBufferPutMore(GPS, buffer, len);
-		case COM_TELEM_UART:
+		case COM_TELEM_USART:
 			return PIOS_USART_TxBufferPutMore(TELEM, buffer, len);
-		case COM_AUX_UART:
+		case COM_AUX_USART:
 			return PIOS_USART_TxBufferPutMore(AUX, buffer, len);
 		case COM_USB_HID:
 			return PIOS_USB_HID_TxBufferPutMore(buffer, len);
@@ -240,21 +240,21 @@ int32_t PIOS_COM_ReceiveHandler(void)
 		/* it would also improve this spagetthi code ;) */
 		int32_t status = -1;
 		switch(intf++) {
-			case COM_DEBUG_UART:
+			case COM_DEBUG_USART:
 				status = PIOS_USART_RxBufferGet(COM_DEBUG_PORT);
-				port = COM_DEBUG_UART;
+				port = COM_DEBUG_USART;
 				break;
-			case COM_GPS_UART:
+			case COM_GPS_USART:
 				status = PIOS_USART_RxBufferGet(GPS);
-				port = COM_GPS_UART;
+				port = COM_GPS_USART;
 				break;
-			case COM_TELEM_UART:
+			case COM_TELEM_USART:
 				status = PIOS_USART_RxBufferGet(TELEM);
-				port = COM_TELEM_UART;
+				port = COM_TELEM_USART;
 				break;
-			case COM_AUX_UART:
+			case COM_AUX_USART:
 				status = PIOS_USART_RxBufferGet(AUX);
-				port = COM_AUX_UART;
+				port = COM_AUX_USART;
 				break;
 			case COM_USB_HID:
 				status = PIOS_USB_HID_RxBufferGet();
diff --git a/flight/PiOS/Common/printf-stdarg.c b/flight/PiOS/Common/printf-stdarg.c
index e4f7c4d02..fa9a26fe7 100644
--- a/flight/PiOS/Common/printf-stdarg.c
+++ b/flight/PiOS/Common/printf-stdarg.c
@@ -46,7 +46,7 @@ static void printchar(char **str, int c)
                 ++(*str);
         }
 
-        else PIOS_COM_SendChar(COM_DEBUG_UART, c); // (void)putchar(c);
+        else PIOS_COM_SendChar(COM_DEBUG_USART, c); // (void)putchar(c);
 
 }
 
diff --git a/flight/PiOS/STM32F10x/pios_sys.c b/flight/PiOS/STM32F10x/pios_sys.c
index bff2ab035..338b2aaef 100644
--- a/flight/PiOS/STM32F10x/pios_sys.c
+++ b/flight/PiOS/STM32F10x/pios_sys.c
@@ -60,16 +60,9 @@ void PIOS_SYS_Init(void)
 	GPIO_Init(GPIOB, &GPIO_InitStructure);
 	GPIO_Init(GPIOD, &GPIO_InitStructure);
 
-	/* Ensure that pull-down is active on USB detach pin */
-	GPIO_InitStructure.GPIO_Pin = 0xffff & ~USB_PULLUP_PIN;
-	GPIO_Init(USB_ACC_GPIO_PORT, &GPIO_InitStructure);
-	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD;
-	GPIO_InitStructure.GPIO_Pin = USB_PULLUP_PIN;
-	GPIO_Init(USB_ACC_GPIO_PORT, &GPIO_InitStructure);
-
-	/* And pull-up for detect pin */
+	/*  Ensure that pull-up is active on detect pin */
 	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
-	GPIO_InitStructure.GPIO_Pin = USB_PULLUP_PIN;
+	GPIO_InitStructure.GPIO_Pin = USB_DETECT_PIN;
 	GPIO_Init(USB_ACC_GPIO_PORT, &GPIO_InitStructure);
 
 	/* Initialize Basic NVIC */
diff --git a/flight/PiOS/STM32F10x/pios_usb.c b/flight/PiOS/STM32F10x/pios_usb.c
index 565a191ac..e526a5e12 100644
--- a/flight/PiOS/STM32F10x/pios_usb.c
+++ b/flight/PiOS/STM32F10x/pios_usb.c
@@ -278,24 +278,6 @@ int32_t PIOS_USB_Init(uint32_t mode)
 		/* Force USB reset and power-down (this will also release the USB pins for direct GPIO control) */
 		_SetCNTR(CNTR_FRES | CNTR_PDWN);
 
-		#if 0
-		/* Disabled because it doesn't work, hardware needs to be looked into */
-		/* Configure USB disconnect pin */
-		/* first we hold it low for ca. 50 mS to force a re-enumeration */
-		GPIO_InitTypeDef GPIO_InitStructure;
-		GPIO_StructInit(&GPIO_InitStructure);
-		GPIO_InitStructure.GPIO_Pin = USB_PULLUP_PIN;
-		GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
-		GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
-		GPIO_Init(USB_ACC_GPIO_PORT, &GPIO_InitStructure);
-
-		GPIO_SetBits(USB_ACC_GPIO_PORT, USB_PULLUP_PIN);
-		PIOS_DELAY_WaitmS(50);
-
-		GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD;
-		GPIO_Init(USB_ACC_GPIO_PORT, &GPIO_InitStructure);
-		#endif
-
 		/* Using a "dirty" method to force a re-enumeration: */
 		/* Force DPM (Pin PA12) low for ca. 10 mS before USB Tranceiver will be enabled */
 		/* This overrules the external Pull-Up at PA12, and at least Windows & MacOS will enumerate again */
@@ -306,8 +288,6 @@ int32_t PIOS_USB_Init(uint32_t mode)
 		GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
 		GPIO_Init(GPIOA, &GPIO_InitStructure);
 
-		GPIO_ResetBits(USB_ACC_GPIO_PORT, USB_PULLUP_PIN);
-
 		PIOS_DELAY_WaitmS(50);
 
 		/* Release power-down, still hold reset */
@@ -386,6 +366,16 @@ int32_t PIOS_USB_IsInitialized(void)
 	return GetEPType(ENDP0) ? 1 : 0;
 }
 
+/**
+* Reads the USB detect pin to determine if a USB cable is connected
+* \return 0 if cable not connected
+* \return 1 if cable is connected
+*/
+int32_t PIOS_USB_CableConnected(void)
+{
+	return GPIO_ReadInputDataBit(USB_ACC_GPIO_PORT, USB_DETECT_PIN);
+}
+
 
 /*
 * Hooks of STM32 USB library
diff --git a/flight/PiOS/inc/pios_board.h b/flight/PiOS/inc/pios_board.h
index 40d97ef7f..94c99dbf7 100644
--- a/flight/PiOS/inc/pios_board.h
+++ b/flight/PiOS/inc/pios_board.h
@@ -242,7 +242,6 @@
 //-------------------------
 #define USB_ACC_GPIO_PORT			GPIOC
 #define USB_DETECT_PIN				GPIO_Pin_4
-#define USB_PULLUP_PIN				GPIO_Pin_14
 #define PIOS_IRQ_USB_PRIORITY			IRQ_PRIO_MID
 
 //-------------------------
diff --git a/flight/PiOS/inc/pios_com.h b/flight/PiOS/inc/pios_com.h
index 36a62f9e7..d5ff89b74 100644
--- a/flight/PiOS/inc/pios_com.h
+++ b/flight/PiOS/inc/pios_com.h
@@ -29,10 +29,10 @@
 
 /* Global Types */
 typedef enum {
-	COM_DEBUG_UART = 0,
-	COM_GPS_UART = 1,
-	COM_TELEM_UART = 2,
-	COM_AUX_UART = 3,
+	COM_DEBUG_USART = 0,
+	COM_GPS_USART = 1,
+	COM_TELEM_USART = 2,
+	COM_AUX_USART = 3,
 	COM_USB_HID = 4
 } COMPortTypeDef;
 
diff --git a/flight/PiOS/inc/pios_usb.h b/flight/PiOS/inc/pios_usb.h
index d1a474de8..6cf1ac88f 100644
--- a/flight/PiOS/inc/pios_usb.h
+++ b/flight/PiOS/inc/pios_usb.h
@@ -71,5 +71,6 @@ extern void (*pEpInt_OUT[7])(void);
 /* Public Functions */
 extern int32_t PIOS_USB_Init(uint32_t mode);
 extern int32_t PIOS_USB_IsInitialized(void);
+extern int32_t PIOS_USB_CableConnected(void);
 
 #endif /* PIOS_USB_H */
diff --git a/flight/PiOS/pios.c b/flight/PiOS/pios.c
index 9ff034873..c0680db10 100644
--- a/flight/PiOS/pios.c
+++ b/flight/PiOS/pios.c
@@ -44,7 +44,7 @@ static uint8_t sdcard_available;
 
 /* Function Prototypes */
 static void TaskTick(void *pvParameters);
-static void TaskHIDTest(void *pvParameters);
+static void TaskTesting(void *pvParameters);
 static void TaskServos(void *pvParameters);
 static void TaskHooks(void *pvParameters);
 static void TaskSDCard(void *pvParameters);
@@ -100,8 +100,6 @@ int main()
 
 	//PIOS_PWM_Init();
 
-	PIOS_USB_Init(0);
-
 	PIOS_COM_ReceiveCallbackInit(CONSOLE_Parse);
 
 	/* Initialise OpenPilot application */
@@ -109,7 +107,7 @@ int main()
 
 	/* Create a FreeRTOS task */
 	xTaskCreate(TaskTick, (signed portCHAR *)"Test", configMINIMAL_STACK_SIZE , NULL, 1, NULL);
-	//xTaskCreate(TaskHIDTest, (signed portCHAR *)"HIDTest", configMINIMAL_STACK_SIZE , NULL, 4, NULL);
+	xTaskCreate(TaskTesting, (signed portCHAR *)"TaskTesting", configMINIMAL_STACK_SIZE , NULL, 4, NULL);
 	//xTaskCreate(TaskServos, (signed portCHAR *)"Servos", configMINIMAL_STACK_SIZE , NULL, 4, NULL);
 	xTaskCreate(TaskHooks, (signed portCHAR *)"Hooks", configMINIMAL_STACK_SIZE, NULL, PRIORITY_TASK_HOOKS, NULL);
 	//xTaskCreate(TaskSDCard, (signed portCHAR *)"SDCard", configMINIMAL_STACK_SIZE, NULL, (tskIDLE_PRIORITY + 2), NULL);
@@ -125,7 +123,7 @@ int main()
 int32_t CONSOLE_Parse(COMPortTypeDef port, char c)
 {
 	if(port == COM_USB_HID) {
-		PIOS_COM_SendChar(COM_DEBUG_UART, c);
+		PIOS_COM_SendChar(COM_DEBUG_USART, c);
 
 		return 0;
 	}
@@ -133,7 +131,7 @@ int32_t CONSOLE_Parse(COMPortTypeDef port, char c)
 	if(c == '\r') {
 		/* Ignore */
 	} else if(c == '\n') {
-		PIOS_COM_SendFormattedString(COM_DEBUG_UART, "String: %s\n", line_buffer);
+		PIOS_COM_SendFormattedString(COM_DEBUG_USART, "String: %s\n", line_buffer);
 		line_ix = 0;
 	} else if(line_ix < (STRING_MAX - 1)) {
 		line_buffer[line_ix++] = c;
@@ -174,36 +172,14 @@ static void TaskTick(void *pvParameters)
 	}
 }
 
-static void TaskHIDTest(void *pvParameters)
+static void TaskTesting(void *pvParameters)
 {
 	portTickType xDelay = 1000 / portTICK_RATE_MS;;
 
-	__IO uint8_t Send_Buffer[63];
-
 	for(;;)
 	{
-
-		Send_Buffer[0] = 'H';
-		Send_Buffer[1] = 'e';
-		Send_Buffer[2] = 'l';
-		Send_Buffer[3] = 'l';
-		Send_Buffer[4] = 'o';
-		Send_Buffer[5] = ' ';
-		Send_Buffer[6] = 'W';
-		Send_Buffer[7] = 'o';
-		Send_Buffer[8] = 'r';
-		Send_Buffer[9] = 'l';
-		Send_Buffer[10] = 'd';
-		Send_Buffer[11] = '!';
-		Send_Buffer[12] = 0;
-
-		/* Write the data to the pipe */
-		//UserToPMABufferCopy((uint8_t*) Send_Buffer, GetEPTxAddr(EP1_IN & 0x7F), 64);
-		//SetEPTxCount(ENDP1, 64);
-
-		//SetEPTxValid(ENDP1);
-
-		PIOS_COM_SendBufferNonBlocking(COM_USB_HID, Send_Buffer, 63);
+		int32_t state = PIOS_USB_CableConnected();
+		PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "State: %d\r", state);
 
 		vTaskDelay(xDelay);
 	}