diff --git a/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_core.h b/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_core.h
index f8bec3af8..4fa53c2c5 100644
--- a/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_core.h
+++ b/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_core.h
@@ -1,8 +1,8 @@
 /******************** (C) COPYRIGHT 2009 STMicroelectronics ********************
 * File Name          : usb_core.h
 * Author             : MCD Application Team
-* Version            : V3.0.1
-* Date               : 04/27/2009
+* Version            : V3.1.0
+* Date               : 10/30/2009
 * Description        : Standard protocol processing functions prototypes
 ********************************************************************************
 * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
@@ -235,8 +235,8 @@ extern  DEVICE  Device_Table;
 extern DEVICE_INFO Device_Info;
 
 /* cells saving status during interrupt servicing */
-extern uint16_t SaveRState;
-extern uint16_t SaveTState;
+extern __IO uint16_t SaveRState;
+extern __IO uint16_t SaveTState;
 
 #endif /* __USB_CORE_H */
 
diff --git a/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_def.h b/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_def.h
index e97d9cba4..9817fb8f4 100644
--- a/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_def.h
+++ b/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_def.h
@@ -1,8 +1,8 @@
 /******************** (C) COPYRIGHT 2009 STMicroelectronics ********************
 * File Name          : usb_def.h
 * Author             : MCD Application Team
-* Version            : V3.0.1
-* Date               : 04/27/2009
+* Version            : V3.1.0
+* Date               : 10/30/2009
 * Description        : Definitions related to USB Core
 ********************************************************************************
 * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
diff --git a/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_init.h b/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_init.h
index 502d926d6..b0a2dd19f 100644
--- a/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_init.h
+++ b/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_init.h
@@ -1,8 +1,8 @@
 /******************** (C) COPYRIGHT 2009 STMicroelectronics ********************
 * File Name          : usb_init.h
 * Author             : MCD Application Team
-* Version            : V3.0.1
-* Date               : 04/27/2009
+* Version            : V3.1.0
+* Date               : 10/30/2009
 * Description        : Initialization routines & global variables
 ********************************************************************************
 * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
diff --git a/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_int.h b/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_int.h
index 6d444b03f..07e981116 100644
--- a/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_int.h
+++ b/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_int.h
@@ -1,8 +1,8 @@
 /******************** (C) COPYRIGHT 2009 STMicroelectronics ********************
 * File Name          : usb_int.h
 * Author             : MCD Application Team
-* Version            : V3.0.1
-* Date               : 04/27/2009
+* Version            : V3.1.0
+* Date               : 10/30/2009
 * Description        : Endpoint CTR (Low and High) interrupt's service routines
 *                      prototypes
 ********************************************************************************
diff --git a/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_lib.h b/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_lib.h
index e534709dd..dbbd82106 100644
--- a/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_lib.h
+++ b/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_lib.h
@@ -1,8 +1,8 @@
 /******************** (C) COPYRIGHT 2009 STMicroelectronics ********************
 * File Name          : usb_lib.h
 * Author             : MCD Application Team
-* Version            : V3.0.1
-* Date               : 04/27/2009
+* Version            : V3.1.0
+* Date               : 10/30/2009
 * Description        : USB library include files
 ********************************************************************************
 * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
@@ -24,8 +24,20 @@
 #include "usb_def.h"
 #include "usb_core.h"
 #include "usb_init.h"
-#include "usb_mem.h"
-#include "usb_int.h"
+#ifndef STM32F10X_CL
+ #include "usb_mem.h"
+ #include "usb_int.h"
+#endif /* STM32F10X_CL */
+
+#include "usb_sil.h"
+
+#ifdef STM32F10X_CL
+ #include "otgd_fs_cal.h"
+ #include "otgd_fs_pcd.h"
+ #include "otgd_fs_dev.h"
+ #include "otgd_fs_int.h"
+#endif /* STM32F10X_CL */
+
 
 /* Exported types ------------------------------------------------------------*/
 /* Exported constants --------------------------------------------------------*/
diff --git a/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_mem.h b/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_mem.h
index a82ea71ff..176374e3d 100644
--- a/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_mem.h
+++ b/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_mem.h
@@ -1,8 +1,8 @@
 /******************** (C) COPYRIGHT 2009 STMicroelectronics ********************
 * File Name          : usb_mem.h
 * Author             : MCD Application Team
-* Version            : V3.0.1
-* Date               : 04/27/2009
+* Version            : V3.1.0
+* Date               : 10/30/2009
 * Description        : Utility prototypes functions for memory/PMA transfers
 ********************************************************************************
 * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
diff --git a/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_regs.h b/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_regs.h
index 4e8fefe83..1f8f4b63a 100644
--- a/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_regs.h
+++ b/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_regs.h
@@ -1,8 +1,8 @@
 /******************** (C) COPYRIGHT 2009 STMicroelectronics ********************
 * File Name          : usb_regs.h
 * Author             : MCD Application Team
-* Version            : V3.0.1
-* Date               : 04/27/2009
+* Version            : V3.1.0
+* Date               : 10/30/2009
 * Description        : Interface prototype functions to USB cell registers
 ********************************************************************************
 * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
@@ -17,6 +17,8 @@
 #ifndef __USB_REGS_H
 #define __USB_REGS_H
 
+#ifndef STM32F10X_CL
+
 /* Includes ------------------------------------------------------------------*/
 /* Exported types ------------------------------------------------------------*/
 typedef enum _EP_DBUF_DIR
@@ -58,15 +60,34 @@ enum EP_BUF_NUM
 /******************************************************************************/
 #define EP0REG  ((__IO unsigned *)(RegBase)) /* endpoint 0 register address */
 
+/* Endpoint Addresses (w/direction) */
+#define EP0_OUT     ((uint8_t)0x00)  
+#define EP0_IN      ((uint8_t)0x80) 
+#define EP1_OUT     ((uint8_t)0x01)  
+#define EP1_IN      ((uint8_t)0x81)  
+#define EP2_OUT     ((uint8_t)0x02)  
+#define EP2_IN      ((uint8_t)0x82)  
+#define EP3_OUT     ((uint8_t)0x03)  
+#define EP3_IN      ((uint8_t)0x83) 
+#define EP4_OUT     ((uint8_t)0x04)  
+#define EP4_IN      ((uint8_t)0x84)
+#define EP5_OUT     ((uint8_t)0x05)  
+#define EP5_IN      ((uint8_t)0x85)
+#define EP6_OUT     ((uint8_t)0x06)  
+#define EP6_IN      ((uint8_t)0x86)
+#define EP7_OUT     ((uint8_t)0x07)  
+#define EP7_IN      ((uint8_t)0x87)
+
 /* endpoints enumeration */
-#define ENDP0   ((uint8_t)0)
-#define ENDP1   ((uint8_t)1)
-#define ENDP2   ((uint8_t)2)
-#define ENDP3   ((uint8_t)3)
-#define ENDP4   ((uint8_t)4)
-#define ENDP5   ((uint8_t)5)
-#define ENDP6   ((uint8_t)6)
-#define ENDP7   ((uint8_t)7)
+#define ENDP0       ((uint8_t)0)
+#define ENDP1       ((uint8_t)1)
+#define ENDP2       ((uint8_t)2)
+#define ENDP3       ((uint8_t)3)
+#define ENDP4       ((uint8_t)4)
+#define ENDP5       ((uint8_t)5)
+#define ENDP6       ((uint8_t)6)
+#define ENDP7       ((uint8_t)7)
+
 /******************************************************************************/
 /*                       ISTR interrupt events                                */
 /******************************************************************************/
@@ -210,12 +231,12 @@ enum EP_BUF_NUM
 * Macro Name     : SetEPType
 * Description    : sets the type in the endpoint register(bits EP_TYPE[1:0])
 * Input          : bEpNum: Endpoint Number. 
-*                  wType
+*                  wType											 
 * Output         : None.
 * Return         : None.
 *******************************************************************************/
 #define _SetEPType(bEpNum,wType) (_SetENDPOINT(bEpNum,\
-                                  ((_GetENDPOINT(bEpNum) & EP_T_MASK) | wType)))
+                                  ((_GetENDPOINT(bEpNum) & EP_T_MASK) | wType )))
 
 /*******************************************************************************
 * Macro Name     : GetEPType
@@ -243,7 +264,7 @@ enum EP_BUF_NUM
     /* toggle second bit ?  */         \
     if((EPTX_DTOG2 & wState)!= 0)      \
       _wRegVal ^= EPTX_DTOG2;        \
-    _SetENDPOINT(bEpNum, _wRegVal);    \
+    _SetENDPOINT(bEpNum, (_wRegVal | EP_CTR_RX|EP_CTR_TX));    \
   } /* _SetEPTxStatus */
 
 /*******************************************************************************
@@ -264,8 +285,36 @@ enum EP_BUF_NUM
     /* toggle second bit ? */  \
     if((EPRX_DTOG2 & wState)!= 0) \
       _wRegVal ^= EPRX_DTOG2;  \
-    _SetENDPOINT(bEpNum, _wRegVal); \
+    _SetENDPOINT(bEpNum, (_wRegVal | EP_CTR_RX|EP_CTR_TX)); \
   } /* _SetEPRxStatus */
+
+/*******************************************************************************
+* Macro Name     : SetEPRxTxStatus
+* Description    : sets the status for rx & tx (bits STAT_TX[1:0] & STAT_RX[1:0])
+* Input          : bEpNum: Endpoint Number. 
+*                  wStaterx: new state.
+*                  wStatetx: new state.
+* Output         : None.
+* Return         : None.
+*******************************************************************************/
+#define _SetEPRxTxStatus(bEpNum,wStaterx,wStatetx) {\
+    register uint32_t _wRegVal;   \
+    \
+    _wRegVal = _GetENDPOINT(bEpNum) & (EPRX_DTOGMASK |EPTX_STAT) ;\
+    /* toggle first bit ? */  \
+    if((EPRX_DTOG1 & wStaterx)!= 0) \
+      _wRegVal ^= EPRX_DTOG1;  \
+    /* toggle second bit ? */  \
+    if((EPRX_DTOG2 & wStaterx)!= 0) \
+      _wRegVal ^= EPRX_DTOG2;  \
+    /* toggle first bit ? */     \
+    if((EPTX_DTOG1 & wStatetx)!= 0)      \
+      _wRegVal ^= EPTX_DTOG1;        \
+    /* toggle second bit ?  */         \
+    if((EPTX_DTOG2 & wStatetx)!= 0)      \
+      _wRegVal ^= EPTX_DTOG2;        \
+    _SetENDPOINT(bEpNum, _wRegVal | EP_CTR_RX|EP_CTR_TX);    \
+  } /* _SetEPRxTxStatus */
 /*******************************************************************************
 * Macro Name     : GetEPTxStatus / GetEPRxStatus 
 * Description    : gets the status for tx/rx transfer (bits STAT_TX[1:0]
@@ -309,9 +358,9 @@ enum EP_BUF_NUM
 * Return         : None.
 *******************************************************************************/
 #define _SetEP_KIND(bEpNum)    (_SetENDPOINT(bEpNum, \
-                                (_GetENDPOINT(bEpNum) | EP_KIND) & EPREG_MASK))
+                                (EP_CTR_RX|EP_CTR_TX|((_GetENDPOINT(bEpNum) | EP_KIND) & EPREG_MASK))))
 #define _ClearEP_KIND(bEpNum)  (_SetENDPOINT(bEpNum, \
-                                (_GetENDPOINT(bEpNum) & EPKIND_MASK)))
+                                (EP_CTR_RX|EP_CTR_TX|(_GetENDPOINT(bEpNum) & EPKIND_MASK))))
 
 /*******************************************************************************
 * Macro Name     : Set_Status_Out / Clear_Status_Out.
@@ -353,9 +402,9 @@ enum EP_BUF_NUM
 * Return         : None.
 *******************************************************************************/
 #define _ToggleDTOG_RX(bEpNum)    (_SetENDPOINT(bEpNum, \
-                                   EP_DTOG_RX | _GetENDPOINT(bEpNum) & EPREG_MASK))
+                                   EP_CTR_RX|EP_CTR_TX|EP_DTOG_RX | (_GetENDPOINT(bEpNum) & EPREG_MASK)))
 #define _ToggleDTOG_TX(bEpNum)    (_SetENDPOINT(bEpNum, \
-                                   EP_DTOG_TX | _GetENDPOINT(bEpNum) & EPREG_MASK))
+                                   EP_CTR_RX|EP_CTR_TX|EP_DTOG_TX | (_GetENDPOINT(bEpNum) & EPREG_MASK)))
 
 /*******************************************************************************
 * Macro Name     : ClearDTOG_RX / ClearDTOG_TX.
@@ -377,7 +426,7 @@ enum EP_BUF_NUM
 * Return         : None.
 *******************************************************************************/
 #define _SetEPAddress(bEpNum,bAddr) _SetENDPOINT(bEpNum,\
-    _GetENDPOINT(bEpNum) & EPREG_MASK | bAddr)
+    EP_CTR_RX|EP_CTR_TX|(_GetENDPOINT(bEpNum) & EPREG_MASK) | bAddr)
 
 /*******************************************************************************
 * Macro Name     : GetEPAddress.
@@ -615,6 +664,8 @@ void FreeUserBuffer(uint8_t bEpNum/*bEpNum*/, uint8_t bDir);
 uint16_t ToWord(uint8_t, uint8_t);
 uint16_t ByteSwap(uint16_t);
 
+#endif /* STM32F10X_CL */
+
 #endif /* __USB_REGS_H */
 
 /******************* (C) COPYRIGHT 2009 STMicroelectronics *****END OF FILE****/
diff --git a/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_sil.h b/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_sil.h
new file mode 100644
index 000000000..974a9a217
--- /dev/null
+++ b/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_sil.h
@@ -0,0 +1,34 @@
+/******************** (C) COPYRIGHT 2009 STMicroelectronics ********************
+* File Name          : usb_sil.h
+* Author             : MCD Application Team
+* Version            : V3.1.0
+* Date               : 10/30/2009
+* Description        : Simplified Interface Layer function prototypes.
+********************************************************************************
+* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
+* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
+* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
+* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
+* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
+* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
+*******************************************************************************/
+
+/* Define to prevent recursive inclusion -------------------------------------*/
+#ifndef __USB_SIL_H
+#define __USB_SIL_H
+
+/* Includes ------------------------------------------------------------------*/
+/* Exported types ------------------------------------------------------------*/
+/* Exported constants --------------------------------------------------------*/
+/* Exported macro ------------------------------------------------------------*/
+/* Exported functions ------------------------------------------------------- */
+
+uint32_t USB_SIL_Init(void);
+uint32_t USB_SIL_Write(uint8_t bEpAddr, uint8_t* pBufferPointer, uint32_t wBufferSize);
+uint32_t USB_SIL_Read(uint8_t bEpAddr, uint8_t* pBufferPointer);
+
+/* External variables --------------------------------------------------------*/
+
+#endif /* __USB_SIL_H */
+
+/******************* (C) COPYRIGHT 2009 STMicroelectronics *****END OF FILE****/
diff --git a/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_type.h b/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_type.h
index a5e423622..54607568b 100644
--- a/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_type.h
+++ b/flight/Libraries/STM32_USB-FS-Device_Driver/inc/usb_type.h
@@ -1,8 +1,8 @@
 /******************** (C) COPYRIGHT 2009 STMicroelectronics ********************
 * File Name          : usb_type.h
 * Author             : MCD Application Team
-* Version            : V3.0.1
-* Date               : 04/27/2009
+* Version            : V3.1.0
+* Date               : 10/30/2009
 * Description        : Type definitions used by the USB Library
 ********************************************************************************
 * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
@@ -18,8 +18,7 @@
 #define __USB_TYPE_H
 
 /* Includes ------------------------------------------------------------------*/
-// TK: disabled, as not required for integration of STM32 drivers!
-// #include "usb_conf.h"
+//#include "usb_conf.h"
 
 /* Exported types ------------------------------------------------------------*/
 /* Exported constants --------------------------------------------------------*/
diff --git a/flight/Libraries/STM32_USB-FS-Device_Driver/src/usb_core.c b/flight/Libraries/STM32_USB-FS-Device_Driver/src/usb_core.c
index 30d5d68a8..4c6723186 100644
--- a/flight/Libraries/STM32_USB-FS-Device_Driver/src/usb_core.c
+++ b/flight/Libraries/STM32_USB-FS-Device_Driver/src/usb_core.c
@@ -1,8 +1,8 @@
 /******************** (C) COPYRIGHT 2009 STMicroelectronics ********************
 * File Name          : usb_core.c
 * Author             : MCD Application Team
-* Version            : V3.0.1
-* Date               : 04/27/2009
+* Version            : V3.1.0
+* Date               : 10/30/2009
 * Description        : Standard protocol processing (USB v2.0)
 ********************************************************************************
 * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
@@ -15,19 +15,19 @@
 
 /* Includes ------------------------------------------------------------------*/
 #include "usb_lib.h"
-//DEVICE_INFO *pInformation;
-//DEVICE Device_Table;
-//DEVICE_PROP *pProperty;
-//USER_STANDARD_REQUESTS *pUser_Standard_Requests;
 /* Private typedef -----------------------------------------------------------*/
 /* Private define ------------------------------------------------------------*/
 #define ValBit(VAR,Place)    (VAR & (1 << Place))
 #define SetBit(VAR,Place)    (VAR |= (1 << Place))
 #define ClrBit(VAR,Place)    (VAR &= ((1 << Place) ^ 255))
 
+#ifdef STM32F10X_CL
+ #define Send0LengthData()  {OTGD_FS_PCD_EP_Write (0, 0, 0) ; vSetEPTxStatus(EP_TX_VALID);}
+#else
 #define Send0LengthData() { _SetEPTxCount(ENDP0, 0); \
     vSetEPTxStatus(EP_TX_VALID); \
   }
+#endif /* STM32F10X_CL */
 
 #define vSetEPRxStatus(st) (SaveRState = st)
 #define vSetEPTxStatus(st) (SaveTState = st)
@@ -41,6 +41,7 @@
 /* Private macro -------------------------------------------------------------*/
 /* Private variables ---------------------------------------------------------*/
 uint16_t_uint8_t StatusInfo;
+
 bool Data_Mul_MaxPacketSize = FALSE;
 /* Private function prototypes -----------------------------------------------*/
 static void DataStageOut(void);
@@ -167,8 +168,8 @@ uint8_t *Standard_GetStatus(uint16_t Length)
     return 0;
   }
 
-  StatusInfo.w = 0;
   /* Reset Status Information */
+  StatusInfo.w = 0;
 
   if (Type_Recipient == (STANDARD_REQUEST | DEVICE_RECIPIENT))
   {
@@ -180,15 +181,19 @@ uint8_t *Standard_GetStatus(uint16_t Length)
     {
       SetBit(StatusInfo0, 1);
     }
+    else
+    {
+      ClrBit(StatusInfo0, 1);
+    }      
 
     /* Bus-powered */
     if (ValBit(Feature, 6))
     {
-      ClrBit(StatusInfo0, 0);
+      SetBit(StatusInfo0, 0);
     }
     else /* Self-powered */
     {
-      SetBit(StatusInfo0, 0);
+      ClrBit(StatusInfo0, 0);
     }
   }
   /*Interface Status*/
@@ -289,7 +294,9 @@ RESULT Standard_ClearFeature(void)
       /* IN endpoint */
       if (_GetTxStallStatus(Related_Endpoint ))
       {
+      #ifndef STM32F10X_CL
         ClearDTOG_TX(Related_Endpoint);
+      #endif /* STM32F10X_CL */
         SetEPTxStatus(Related_Endpoint, EP_TX_VALID);
       }
     }
@@ -301,14 +308,15 @@ RESULT Standard_ClearFeature(void)
         if (Related_Endpoint == ENDP0)
         {
           /* After clear the STALL, enable the default endpoint receiver */
-//          SetEPRxCount(Related_Endpoint, Device_Property.MaxPacketSize);
-// TK: we should reference MaxPacketSize() via pointer!
+          //SetEPRxCount(Related_Endpoint, Device_Property.MaxPacketSize);
           SetEPRxCount(Related_Endpoint, pProperty->MaxPacketSize);
           _SetEPRxStatus(Related_Endpoint, EP_RX_VALID);
         }
         else
         {
+        #ifndef STM32F10X_CL
           ClearDTOG_RX(Related_Endpoint);
+        #endif /* STM32F10X_CL */
           _SetEPRxStatus(Related_Endpoint, EP_RX_VALID);
         }
       }
@@ -333,7 +341,7 @@ RESULT Standard_SetEndPointFeature(void)
   uint32_t    wIndex0;
   uint32_t    Related_Endpoint;
   uint32_t    rEP;
-  uint32_t   Status;
+  uint32_t    Status;
 
   wIndex0 = pInformation->USBwIndex0;
   rEP = wIndex0 & ~0x80;
@@ -451,7 +459,11 @@ void DataStageOut(void)
     pEPinfo->Usb_rLength -= Length;
     pEPinfo->Usb_rOffset += Length;
 
+  #ifdef STM32F10X_CL  
+    OTGD_FS_PCD_EP_Read(ENDP0, Buffer, Length); 
+  #else  
     PMAToUserBufferCopy(Buffer, GetEPRxAddr(ENDP0), Length);
+  #endif  /* STM32F10X_CL */
   }
 
   if (pEPinfo->Usb_rLength != 0)
@@ -508,7 +520,14 @@ void DataStageIn(void)
     {
       /* No more data to send so STALL the TX Status*/
       ControlState = WAIT_STATUS_OUT;
+
+    #ifdef STM32F10X_CL      
+      OTGD_FS_PCD_EP_Read (ENDP0, 0, 0);
+    #endif  /* STM32F10X_CL */ 
+    
+    #ifndef STM32F10X_CL 
       vSetEPTxStatus(EP_TX_STALL);
+    #endif  /* STM32F10X_CL */ 
     }
     
     goto Expect_Status_Out;
@@ -524,7 +543,11 @@ void DataStageIn(void)
 
   DataBuffer = (*pEPinfo->CopyData)(Length);
 
+#ifdef STM32F10X_CL
+  OTGD_FS_PCD_EP_Write (ENDP0, DataBuffer, Length);
+#else   
   UserToPMABufferCopy(DataBuffer, GetEPTxAddr(ENDP0), Length);
+#endif /* STM32F10X_CL */ 
 
   SetEPTxCount(ENDP0, Length);
 
@@ -574,6 +597,10 @@ void NoData_Setup0(void)
       else
       {
         Result = USB_SUCCESS;
+
+      #ifdef STM32F10X_CL
+         SetDeviceAddress(pInformation->USBwValue0);
+      #endif  /* STM32F10X_CL */
       }
     }
     /*SET FEATURE for Device*/
@@ -683,6 +710,7 @@ void Data_Setup0(void)
   CopyRoutine = NULL;
   wOffset = 0;
 
+  /*GET DESCRIPTOR*/
   if (Request_No == GET_DESCRIPTOR)
   {
     if (Type_Recipient == (STANDARD_REQUEST | DEVICE_RECIPIENT))
@@ -810,7 +838,7 @@ void Data_Setup0(void)
     /* Device ==> Host */
     __IO uint32_t wLength = pInformation->USBwLength;
      
-    /* Restrict the data length to be the one host asks */
+    /* Restrict the data length to be the one host asks for */
     if (pInformation->Ctrl_Info.Usb_wLength > wLength)
     {
       pInformation->Ctrl_Info.Usb_wLength = wLength;
@@ -856,17 +884,29 @@ uint8_t Setup0_Process(void)
     uint16_t* w;
   } pBuf;
 
+#ifdef STM32F10X_CL
+  USB_OTG_EP *ep;
+  uint16_t offset = 0;
+ 
+  ep = OTGD_FS_PCD_GetOutEP(ENDP0);
+  pBuf.b = ep->xfer_buff;
+  
+  OTGD_FS_EP0StartXfer(ep);
+#else  
+  uint16_t offset = 1;
+  
   pBuf.b = PMAAddr + (uint8_t *)(_GetEPRxAddr(ENDP0) * 2); /* *2 for 32 bits addr */
+#endif /* STM32F10X_CL */
 
   if (pInformation->ControlState != PAUSE)
   {
     pInformation->USBbmRequestType = *pBuf.b++; /* bmRequestType */
     pInformation->USBbRequest = *pBuf.b++; /* bRequest */
-    pBuf.w++;  /* word not accessed because of 32 bits addressing */
+    pBuf.w += offset;  /* word not accessed because of 32 bits addressing */
     pInformation->USBwValue = ByteSwap(*pBuf.w++); /* wValue */
-    pBuf.w++;  /* word not accessed because of 32 bits addressing */
+    pBuf.w += offset;  /* word not accessed because of 32 bits addressing */
     pInformation->USBwIndex  = ByteSwap(*pBuf.w++); /* wIndex */
-    pBuf.w++;  /* word not accessed because of 32 bits addressing */
+    pBuf.w += offset;  /* word not accessed because of 32 bits addressing */
     pInformation->USBwLength = *pBuf.w; /* wLength */
   }
 
@@ -935,7 +975,12 @@ uint8_t Out0_Process(void)
 {
   uint32_t ControlState = pInformation->ControlState;
 
-  if ((ControlState == OUT_DATA) || (ControlState == LAST_OUT_DATA))
+  if ((ControlState == IN_DATA) || (ControlState == LAST_IN_DATA))
+  {
+    /* host aborts the transfer before finish */
+    ControlState = STALLED;
+  }
+  else if ((ControlState == OUT_DATA) || (ControlState == LAST_OUT_DATA))
   {
     DataStageOut();
     ControlState = pInformation->ControlState; /* may be changed outside the function */
@@ -944,14 +989,11 @@ uint8_t Out0_Process(void)
   else if (ControlState == WAIT_STATUS_OUT)
   {
     (*pProperty->Process_Status_OUT)();
+  #ifndef STM32F10X_CL
     ControlState = STALLED;
+  #endif /* STM32F10X_CL */
   }
 
-  else if ((ControlState == IN_DATA) || (ControlState == LAST_IN_DATA))
-  {
-    /* host aborts the transfer before finish */
-    ControlState = STALLED;
-  }
 
   /* Unexpect state, STALL the endpoint */
   else
@@ -974,8 +1016,11 @@ uint8_t Out0_Process(void)
 *******************************************************************************/
 uint8_t Post0_Process(void)
 {
-  //  SetEPRxCount(ENDP0, Device_Property.MaxPacketSize);
-// TK: we should reference to MaxPacketSize() via pointer!
+#ifdef STM32F10X_CL  
+  USB_OTG_EP *ep;
+#endif /* STM32F10X_CL */
+      
+  //SetEPRxCount(ENDP0, Device_Property.MaxPacketSize);
   SetEPRxCount(ENDP0, pProperty->MaxPacketSize);
 
   if (pInformation->ControlState == STALLED)
@@ -984,6 +1029,26 @@ uint8_t Post0_Process(void)
     vSetEPTxStatus(EP_TX_STALL);
   }
 
+#ifdef STM32F10X_CL
+  else if ((pInformation->ControlState == OUT_DATA) ||
+      (pInformation->ControlState == WAIT_STATUS_OUT))
+  {
+    ep = OTGD_FS_PCD_GetInEP(0);
+    ep->is_in = 0;
+    OTGD_FS_EP0StartXfer(ep);
+    
+    vSetEPTxStatus(EP_TX_VALID);
+  }
+  
+  else if ((pInformation->ControlState == IN_DATA) || 
+      (pInformation->ControlState == WAIT_STATUS_IN))
+  {
+    ep = OTGD_FS_PCD_GetInEP(0);
+    ep->is_in = 1;
+    OTGD_FS_EP0StartXfer(ep);    
+  }  
+#endif /* STM32F10X_CL */
+
   return (pInformation->ControlState == PAUSE);
 }
 
@@ -996,6 +1061,9 @@ uint8_t Post0_Process(void)
 *******************************************************************************/
 void SetDeviceAddress(uint8_t Val)
 {
+#ifdef STM32F10X_CL 
+  OTGD_FS_PCD_EP_SetAddress ((uint8_t)Val);
+#else 
   uint32_t i;
   uint32_t nEP = Device_Table.Total_Endpoint;
 
@@ -1005,6 +1073,7 @@ void SetDeviceAddress(uint8_t Val)
     _SetEPAddress((uint8_t)i, (uint8_t)i);
   } /* for */
   _SetDADDR(Val | DADDR_EF); /* set device address and enable function */
+#endif  /* STM32F10X_CL */  
 }
 
 /*******************************************************************************
diff --git a/flight/Libraries/STM32_USB-FS-Device_Driver/src/usb_init.c b/flight/Libraries/STM32_USB-FS-Device_Driver/src/usb_init.c
index 87aa93413..e7a250978 100644
--- a/flight/Libraries/STM32_USB-FS-Device_Driver/src/usb_init.c
+++ b/flight/Libraries/STM32_USB-FS-Device_Driver/src/usb_init.c
@@ -1,8 +1,8 @@
 /******************** (C) COPYRIGHT 2009 STMicroelectronics ********************
 * File Name          : usb_init.c
 * Author             : MCD Application Team
-* Version            : V3.0.1
-* Date               : 04/27/2009
+* Version            : V3.1.0
+* Date               : 10/30/2009
 * Description        : Initialization routines & global variables
 ********************************************************************************
 * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
diff --git a/flight/Libraries/STM32_USB-FS-Device_Driver/src/usb_int.c b/flight/Libraries/STM32_USB-FS-Device_Driver/src/usb_int.c
index bc10f1080..68e2fc157 100644
--- a/flight/Libraries/STM32_USB-FS-Device_Driver/src/usb_int.c
+++ b/flight/Libraries/STM32_USB-FS-Device_Driver/src/usb_int.c
@@ -1,8 +1,8 @@
 /******************** (C) COPYRIGHT 2009 STMicroelectronics ********************
 * File Name          : usb_int.c
 * Author             : MCD Application Team
-* Version            : V3.0.1
-* Date               : 04/27/2009
+* Version            : V3.1.0
+* Date               : 10/30/2009
 * Description        : Endpoint CTR (Low and High) interrupt's service routines
 ********************************************************************************
 * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
@@ -12,6 +12,7 @@
 * CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
 * INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
 *******************************************************************************/
+#ifndef STM32F10X_CL
 
 /* Includes ------------------------------------------------------------------*/
 #include "usb_lib.h"
@@ -20,8 +21,8 @@
 /* Private define ------------------------------------------------------------*/
 /* Private macro -------------------------------------------------------------*/
 /* Private variables ---------------------------------------------------------*/
-uint16_t SaveRState;
-uint16_t SaveTState;
+__IO uint16_t SaveRState;
+__IO uint16_t SaveTState;
 
 /* Extern variables ----------------------------------------------------------*/
 extern void (*pEpInt_IN[7])(void);    /*  Handles IN  interrupts   */
@@ -40,20 +41,11 @@ extern void (*pEpInt_OUT[7])(void);   /*  Handles OUT interrupts   */
 *******************************************************************************/
 void CTR_LP(void)
 {
-  uint32_t wEPVal = 0;
-
-  // TK: inserted, was a global variable before which could lead to unintended overwrites
-  // if CTR_LP() was called from a different task
   uint16_t wIstr;
-
-  // TK: same for this global variable, we have a conflict if CTR_LP() and CTR_HP()
-  // are called with different priorities
-  uint8_t EPindex;
-
+  __IO uint16_t wEPVal = 0;
   /* stay in loop while pending ints */
   while (((wIstr = _GetISTR()) & ISTR_CTR) != 0)
   {
-    _SetISTR((uint16_t)CLR_CTR); /* clear CTR flag */
     /* extract highest priority endpoint number */
     EPindex = (uint8_t)(wIstr & ISTR_EP_ID);
     if (EPindex == 0)
@@ -64,12 +56,14 @@ void CTR_LP(void)
 
       /* save RX & TX status */
       /* and set both to NAK */
-      SaveRState = _GetEPRxStatus(ENDP0);
-      SaveTState = _GetEPTxStatus(ENDP0);
-      _SetEPRxStatus(ENDP0, EP_RX_NAK);
-      _SetEPTxStatus(ENDP0, EP_TX_NAK);
 
 
+	    SaveRState = _GetENDPOINT(ENDP0);
+	    SaveTState = SaveRState & EPTX_STAT;
+	    SaveRState &=  EPRX_STAT;	
+
+	    _SetEPRxTxStatus(ENDP0,EP_RX_NAK,EP_TX_NAK);
+
       /* DIR bit = origin of the interrupt */
 
       if ((wIstr & ISTR_DIR) == 0)
@@ -84,9 +78,9 @@ void CTR_LP(void)
         In0_Process();
 
            /* before terminate set Tx & Rx status */
-          _SetEPRxStatus(ENDP0, SaveRState);
-          _SetEPTxStatus(ENDP0, SaveTState);
-          return;
+
+            _SetEPRxTxStatus(ENDP0,SaveRState,SaveTState);
+		  return;
       }
       else
       {
@@ -96,22 +90,14 @@ void CTR_LP(void)
         /* DIR = 1 & (CTR_TX | CTR_RX) => 2 int pending */
 
         wEPVal = _GetENDPOINT(ENDP0);
-        if ((wEPVal & EP_CTR_TX) != 0)
-        {
-          _ClearEP_CTR_TX(ENDP0);
-          In0_Process();
-          /* before terminate set Tx & Rx status */
-          _SetEPRxStatus(ENDP0, SaveRState);
-          _SetEPTxStatus(ENDP0, SaveTState);
-          return;
-        }
-        else if ((wEPVal &EP_SETUP) != 0)
+        
+        if ((wEPVal &EP_SETUP) != 0)
         {
           _ClearEP_CTR_RX(ENDP0); /* SETUP bit kept frozen while CTR_RX = 1 */
           Setup0_Process();
           /* before terminate set Tx & Rx status */
-          _SetEPRxStatus(ENDP0, SaveRState);
-          _SetEPTxStatus(ENDP0, SaveTState);
+
+		      _SetEPRxTxStatus(ENDP0,SaveRState,SaveTState);
           return;
         }
 
@@ -120,8 +106,8 @@ void CTR_LP(void)
           _ClearEP_CTR_RX(ENDP0);
           Out0_Process();
           /* before terminate set Tx & Rx status */
-          _SetEPRxStatus(ENDP0, SaveRState);
-          _SetEPTxStatus(ENDP0, SaveTState);
+     
+		     _SetEPRxTxStatus(ENDP0,SaveRState,SaveTState);
           return;
         }
       }
@@ -166,12 +152,8 @@ void CTR_LP(void)
 *******************************************************************************/
 void CTR_HP(void)
 {
-  uint32_t wEPVal = 0;
-
-  // TK: made local - we have a conflict if CTR_LP() and CTR_HP()
-  // are called with different priorities
-  uint8_t EPindex;
   uint16_t wIstr;
+  uint32_t wEPVal = 0;
 
   while (((wIstr = _GetISTR()) & ISTR_CTR) != 0)
   {
@@ -203,4 +185,6 @@ void CTR_HP(void)
   }/* while(...) */
 }
 
+#endif  /* STM32F10X_CL */
+
 /******************* (C) COPYRIGHT 2009 STMicroelectronics *****END OF FILE****/
diff --git a/flight/Libraries/STM32_USB-FS-Device_Driver/src/usb_mem.c b/flight/Libraries/STM32_USB-FS-Device_Driver/src/usb_mem.c
index 9867fd942..5a03910a0 100644
--- a/flight/Libraries/STM32_USB-FS-Device_Driver/src/usb_mem.c
+++ b/flight/Libraries/STM32_USB-FS-Device_Driver/src/usb_mem.c
@@ -1,8 +1,8 @@
 /******************** (C) COPYRIGHT 2009 STMicroelectronics ********************
 * File Name          : usb_mem.c
 * Author             : MCD Application Team
-* Version            : V3.0.1
-* Date               : 04/27/2009
+* Version            : V3.1.0
+* Date               : 10/30/2009
 * Description        : Utility functions for memory transfers to/from PMA
 ********************************************************************************
 * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
@@ -12,6 +12,7 @@
 * CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
 * INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
 *******************************************************************************/
+#ifndef STM32F10X_CL
 
 /* Includes ------------------------------------------------------------------*/
 #include "usb_lib.h"
@@ -70,4 +71,5 @@ void PMAToUserBufferCopy(uint8_t *pbUsrBuf, uint16_t wPMABufAddr, uint16_t wNByt
   }
 }
 
+#endif /* STM32F10X_CL */
 /******************* (C) COPYRIGHT 2009 STMicroelectronics *****END OF FILE****/
diff --git a/flight/Libraries/STM32_USB-FS-Device_Driver/src/usb_regs.c b/flight/Libraries/STM32_USB-FS-Device_Driver/src/usb_regs.c
index 137674130..38e8bbb1a 100644
--- a/flight/Libraries/STM32_USB-FS-Device_Driver/src/usb_regs.c
+++ b/flight/Libraries/STM32_USB-FS-Device_Driver/src/usb_regs.c
@@ -1,8 +1,8 @@
 /******************** (C) COPYRIGHT 2009 STMicroelectronics ********************
 * File Name          : usb_regs.c
 * Author             : MCD Application Team
-* Version            : V3.0.1
-* Date               : 04/27/2009
+* Version            : V3.1.0
+* Date               : 10/30/2009
 * Description        : Interface functions to USB cell registers
 ********************************************************************************
 * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
@@ -12,6 +12,7 @@
 * CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
 * INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
 *******************************************************************************/
+#ifndef STM32F10X_CL
 
 /* Includes ------------------------------------------------------------------*/
 #include "usb_lib.h"
@@ -745,4 +746,5 @@ uint16_t ByteSwap(uint16_t wSwW)
   return(wRet);
 }
 
+#endif /* STM32F10X_CL */
 /******************* (C) COPYRIGHT 2009 STMicroelectronics *****END OF FILE****/
diff --git a/flight/Libraries/STM32_USB-FS-Device_Driver/src/usb_sil.c b/flight/Libraries/STM32_USB-FS-Device_Driver/src/usb_sil.c
new file mode 100644
index 000000000..00eb71c7d
--- /dev/null
+++ b/flight/Libraries/STM32_USB-FS-Device_Driver/src/usb_sil.c
@@ -0,0 +1,126 @@
+/******************** (C) COPYRIGHT 2009 STMicroelectronics ********************
+* File Name          : usb_sil.c
+* Author             : MCD Application Team
+* Version            : V3.1.0
+* Date               : 10/30/2009
+* Description        : Simplified Interface Layer for Global Initialization and 
+*                      Endpoint Rea/Write operations.
+********************************************************************************
+* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
+* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
+* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
+* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
+* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
+* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
+*******************************************************************************/
+
+/* Includes ------------------------------------------------------------------*/
+#include "usb_lib.h"
+
+/* Private typedef -----------------------------------------------------------*/
+/* Private define ------------------------------------------------------------*/
+/* Private macro -------------------------------------------------------------*/
+/* Private variables ---------------------------------------------------------*/
+/* Extern variables ----------------------------------------------------------*/
+/* Private function prototypes -----------------------------------------------*/
+/* Private functions ---------------------------------------------------------*/
+
+/*******************************************************************************
+* Function Name  : USB_SIL_Init
+* Description    : Initialize the USB Device IP and the Endpoint 0.
+* Input          : None.
+* Output         : None.
+* Return         : Status.
+*******************************************************************************/
+uint32_t USB_SIL_Init(void)
+{
+#ifndef STM32F10X_CL
+  
+  /* USB interrupts initialization */
+  /* clear pending interrupts */
+  _SetISTR(0);
+  wInterrupt_Mask = (CNTR_CTRM  | CNTR_WKUPM | CNTR_SUSPM | CNTR_ERRM  | CNTR_SOFM | CNTR_ESOFM | CNTR_RESETM);
+  /* set interrupts mask */
+  _SetCNTR(wInterrupt_Mask);
+  
+#else
+  
+  /* Perform OTG Device initialization procedure (including EP0 init) */
+  OTG_DEV_Init();
+  
+#endif /* STM32F10X_CL */
+
+  return 0;
+}
+
+/*******************************************************************************
+* Function Name  : USB_SIL_Write
+* Description    : Write a buffer of data to a selected endpoint.
+* Input          : - bEpAddr: The address of the non control endpoint.
+*                  - pBufferPointer: The pointer to the buffer of data to be written
+*                    to the endpoint.
+*                  - wBufferSize: Number of data to be written (in bytes).
+* Output         : None.
+* Return         : Status.
+*******************************************************************************/
+uint32_t USB_SIL_Write(uint8_t bEpAddr, uint8_t* pBufferPointer, uint32_t wBufferSize)
+{
+#ifndef STM32F10X_CL
+
+  /* Use the memory interface function to write to the selected endpoint */
+  UserToPMABufferCopy(pBufferPointer, GetEPTxAddr(bEpAddr & 0x7F), wBufferSize);
+
+  /* Update the data length in the control register */
+  SetEPTxCount((bEpAddr & 0x7F), wBufferSize);
+  
+#else
+  
+   /* Use the PCD interface layer function to write to the selected endpoint */
+   OTGD_FS_PCD_EP_Write (bEpAddr, pBufferPointer, wBufferSize); 
+   
+#endif /* STM32F10X_CL */
+
+  return 0;
+}
+
+/*******************************************************************************
+* Function Name  : USB_SIL_Read
+* Description    : Write a buffer of data to a selected endpoint.
+* Input          : - bEpAddr: The address of the non control endpoint.
+*                  - pBufferPointer: The pointer to which will be saved the 
+*                     received data buffer.
+* Output         : None.
+* Return         : Number of received data (in Bytes).
+*******************************************************************************/
+uint32_t USB_SIL_Read(uint8_t bEpAddr, uint8_t* pBufferPointer)
+{
+  uint32_t DataLength = 0;
+
+#ifndef STM32F10X_CL
+
+  /* Get the number of received data on the selected Endpoint */
+  DataLength = GetEPRxCount(bEpAddr & 0x7F);
+  
+  /* Use the memory interface function to write to the selected endpoint */
+  PMAToUserBufferCopy(pBufferPointer, GetEPRxAddr(bEpAddr & 0x7F), DataLength);
+
+#else
+  
+  USB_OTG_EP *ep;
+
+  /* Get the structure pointer of the selected Endpoint */
+  ep = OTGD_FS_PCD_GetOutEP(bEpAddr);
+  
+  /* Get the number of received data */
+  DataLength = ep->xfer_len;
+  
+  /* Use the PCD interface layer function to read the selected endpoint */
+  OTGD_FS_PCD_EP_Read (bEpAddr, pBufferPointer, DataLength);
+  
+#endif /* STM32F10X_CL */
+
+  /* Return the number of received data */
+  return DataLength;
+}
+
+/******************* (C) COPYRIGHT 2009 STMicroelectronics *****END OF FILE****/
diff --git a/flight/Libraries/msd/msd.c b/flight/Libraries/msd/msd.c
index a0a5422d2..5355f1aee 100644
--- a/flight/Libraries/msd/msd.c
+++ b/flight/Libraries/msd/msd.c
@@ -72,13 +72,25 @@ static const DEVICE_PROP My_Device_Property = {
 		MSD_MASS_Reset, MSD_MASS_Status_In, MSD_MASS_Status_Out, MSD_MASS_Data_Setup, MSD_MASS_NoData_Setup, MSD_MASS_Get_Interface_Setting,
 		MSD_MASS_GetDeviceDescriptor, MSD_MASS_GetConfigDescriptor, MSD_MASS_GetStringDescriptor, 0, 0x40 /*MAX PACKET SIZE*/
 };
-static const USER_STANDARD_REQUESTS My_User_Standard_Requests = {NOP_Process, MSD_Mass_Storage_SetConfiguration, NOP_Process, NOP_Process, NOP_Process,
-		MSD_Mass_Storage_ClearFeature, NOP_Process, NOP_Process, MSD_Mass_Storage_SetDeviceAddress};
+static const USER_STANDARD_REQUESTS My_User_Standard_Requests = {
+		NOP_Process,
+		MSD_Mass_Storage_SetConfiguration,
+		NOP_Process,
+		NOP_Process,
+		NOP_Process,
+		MSD_Mass_Storage_ClearFeature,
+		NOP_Process, NOP_Process,
+		MSD_Mass_Storage_SetDeviceAddress
+};
 static ONE_DESCRIPTOR Device_Descriptor = {(uint8_t *) MSD_MASS_DeviceDescriptor, MSD_MASS_SIZ_DEVICE_DESC};
 static ONE_DESCRIPTOR Config_Descriptor = {(uint8_t *) MSD_MASS_ConfigDescriptor, MSD_MASS_SIZ_CONFIG_DESC};
-static ONE_DESCRIPTOR String_Descriptor[5] = { {(uint8_t *) MSD_MASS_StringLangID, MSD_MASS_SIZ_STRING_LANGID}, {(uint8_t *) MSD_MASS_StringVendor,
-		MSD_MASS_SIZ_STRING_VENDOR}, {(uint8_t *) MSD_MASS_StringProduct, MSD_MASS_SIZ_STRING_PRODUCT}, {(uint8_t *) MSD_MASS_StringSerial,
-		MSD_MASS_SIZ_STRING_SERIAL}, {(uint8_t *) MSD_MASS_StringInterface, MSD_MASS_SIZ_STRING_INTERFACE}, };
+static ONE_DESCRIPTOR String_Descriptor[5] = {
+		{(uint8_t *) MSD_MASS_StringLangID, MSD_MASS_SIZ_STRING_LANGID},
+		{(uint8_t *) MSD_MASS_StringVendor, MSD_MASS_SIZ_STRING_VENDOR},
+		{(uint8_t *) MSD_MASS_StringProduct, MSD_MASS_SIZ_STRING_PRODUCT},
+		{(uint8_t *) MSD_MASS_StringSerial, MSD_MASS_SIZ_STRING_SERIAL},
+		{(uint8_t *) MSD_MASS_StringInterface, MSD_MASS_SIZ_STRING_INTERFACE},
+};
 static uint8_t lun_available;
 
 /**
diff --git a/flight/Makefile b/flight/Makefile
index b8b969ae6..2c70fc0f6 100644
--- a/flight/Makefile
+++ b/flight/Makefile
@@ -106,6 +106,7 @@ SRC += $(PIOSSTM32F10X)/pios_i2c.c
 SRC += $(PIOSSTM32F10X)/pios_spi.c
 SRC += $(PIOSSTM32F10X)/pios_pwm.c
 SRC += $(PIOSSTM32F10X)/pios_usb.c
+SRC += $(PIOSSTM32F10X)/pios_usb_hid.c
 
 ## PIOS Hardware (Common)
 SRC += $(PIOSCOMMON)/pios_sdcard.c
@@ -142,6 +143,7 @@ SRC += $(STMUSBSRCDIR)/usb_init.c
 SRC += $(STMUSBSRCDIR)/usb_int.c
 SRC += $(STMUSBSRCDIR)/usb_mem.c
 SRC += $(STMUSBSRCDIR)/usb_regs.c
+SRC += $(STMUSBSRCDIR)/usb_sil.c
 
 ## RTOS
 SRC += $(RTOSSRCDIR)/list.c 
diff --git a/flight/PiOS/STM32F10x/pios_usb.c b/flight/PiOS/STM32F10x/pios_usb.c
index 21ef0131f..7289396f2 100644
--- a/flight/PiOS/STM32F10x/pios_usb.c
+++ b/flight/PiOS/STM32F10x/pios_usb.c
@@ -28,14 +28,9 @@
 
 /* Project Includes */
 #include "pios.h"
-
-/* Private Function Prototypes */
-
-/* Local Variables */
-
-#include <usb_lib.h>
 #include <string.h>
 
+
 /* Local definitions */
 #define DSCR_DEVICE     1       /* Descriptor type: Device */
 #define DSCR_CONFIG     2       /* Descriptor type: Configuration */
@@ -44,13 +39,19 @@
 #define DSCR_ENDPNT     5       /* Descriptor type: Endpoint */
 #define CS_INTERFACE    0x24    /* Class-specific type: Interface */
 #define CS_ENDPOINT     0x25    /* Class-specific type: Endpoint */
+
 /* ISTR events */
 /* mask defining which events has to be handled by the device application software */
-#define IMR_MSK (CNTR_CTRM | CNTR_RESETM)
+#define IMR_MSK (CNTR_CTRM  | CNTR_WKUPM | CNTR_SUSPM | CNTR_ERRM  | CNTR_SOFM | CNTR_ESOFM | CNTR_RESETM)
 
 /* Local types */
 typedef enum _DEVICE_STATE {
-	UNCONNECTED, ATTACHED, POWERED, SUSPENDED, ADDRESSED, CONFIGURED
+	UNCONNECTED,
+	ATTACHED,
+	POWERED,
+	SUSPENDED,
+	ADDRESSED,
+	CONFIGURED
 } DEVICE_STATE;
 
 /* Global Variables used by STM32 USB Driver */
@@ -63,23 +64,42 @@ DEVICE_PROP *pProperty;
 USER_STANDARD_REQUESTS *pUser_Standard_Requests;
 
 /* Stored in RAM, vectors can be changed on-the-fly */
-void (*pEpInt_IN[7])(void) = {NOP_Process, NOP_Process, NOP_Process, NOP_Process, NOP_Process, NOP_Process, NOP_Process};
-void (*pEpInt_OUT[7])(void) = {NOP_Process, NOP_Process, NOP_Process, NOP_Process, NOP_Process, NOP_Process, NOP_Process};
+void (*pEpInt_IN[7])(void) = {
+		NOP_Process,
+		NOP_Process,
+		NOP_Process,
+		NOP_Process,
+		NOP_Process,
+		NOP_Process,
+		NOP_Process
+		};
+void (*pEpInt_OUT[7])(void) = {
+		NOP_Process,
+		NOP_Process,
+		NOP_Process,
+		NOP_Process,
+		NOP_Process,
+		NOP_Process,
+		NOP_Process
+		};
 
-#define PIOS_USB_NUM_INTERFACES              (0)
-#define PIOS_USB_SIZ_CONFIG_DESC             (9 + 0)
+#define USB_ENDPOINT_DESCRIPTOR_TYPE		0x05
+
+#define PIOS_USB_HID_NUM_INTERFACES		1
+#define PIOS_USB_HID_SIZ_CLASS_DESC		18
+#define PIOS_USB_HID_SIZ_CONFIG_DESC		32
+
+#define PIOS_USB_NUM_INTERFACES			(PIOS_USB_HID_NUM_INTERFACES)
+#define PIOS_USB_SIZ_CONFIG_DESC		(9 + PIOS_USB_HID_SIZ_CONFIG_DESC)
 
 /* USB Standard Device Descriptor */
 #define PIOS_USB_SIZ_DEVICE_DESC 18
-static const uint8_t PIOS_USB_DeviceDescriptor[PIOS_USB_SIZ_DEVICE_DESC] = {(uint8_t)(PIOS_USB_SIZ_DEVICE_DESC & 0xff), /* Device Descriptor length */
+static const uint8_t PIOS_USB_DeviceDescriptor[PIOS_USB_SIZ_DEVICE_DESC] = {
+		(uint8_t)(PIOS_USB_SIZ_DEVICE_DESC & 0xff), /* Device Descriptor length */
 		DSCR_DEVICE, /* Descriptor type */
 		(uint8_t)(0x0200 & 0xff), /* Specification Version (BCD, LSB) */
 		(uint8_t)(0x0200 >> 8), /* Specification Version (BCD, MSB) */
-#if 1
-		0x02, /* Device class "Communication"   -- required for MacOS to find the COM device. Audio Device works fine in parallel to this */
-#else
-		0x00, /* Device class "Composite" */
-#endif
+		0x00, /* Device class "Communication" */
 		0x00, /* Device sub-class */
 		0x00, /* Device sub-sub-class */
 		0x40, /* Maximum packet size */
@@ -95,7 +115,7 @@ static const uint8_t PIOS_USB_DeviceDescriptor[PIOS_USB_SIZ_DEVICE_DESC] = {(uin
 		0x01 /* Number of configurations */
 		};
 
-/* USB Config Descriptor */
+/* USB Configuration Descriptor */
 static const uint8_t PIOS_USB_ConfigDescriptor[PIOS_USB_SIZ_CONFIG_DESC] = {
 		/* Configuration Descriptor */
 		9, /* Descriptor length */
@@ -108,9 +128,51 @@ static const uint8_t PIOS_USB_ConfigDescriptor[PIOS_USB_SIZ_CONFIG_DESC] = {
 		0x80, /* Attributes (b7 - buspwr, b6 - selfpwr, b5 - rwu) */
 		0x32, /* Power requirement (div 2 ma) */
 
-		/* TODO:HID */
+		/* HID */
+		/************** Descriptor of Custom HID interface ****************/
+		/* 09 */
+		0x09, /* bLength: Interface Descriptor size */
+		0x04,/* bDescriptorType: Interface descriptor type */
+		0x00, /* bInterfaceNumber: Number of Interface */
+		0x00, /* bAlternateSetting: Alternate setting */
+		0x02, /* bNumEndpoints */
+		0x03, /* bInterfaceClass: HID */
+		0x00, /* bInterfaceSubClass : 1=BOOT, 0=no boot */
+		0x00, /* nInterfaceProtocol : 0=none, 1=keyboard, 2=mouse */
+		0, /* iInterface: Index of string descriptor */
 
-		};
+		/******************** Descriptor of Custom HID HID ********************/
+		/* 18 */
+		0x09, /* bLength: HID Descriptor size */
+		0x21, /* bDescriptorType: HID */
+		0x10, /* bcdHID: HID Class Spec release number */
+		0x01, 0x00, /* bCountryCode: Hardware target country */
+		0x01, /* bNumDescriptors: Number of HID class descriptors to follow */
+		0x22, /* bDescriptorType */
+		PIOS_USB_HID_SIZ_REPORT_DESC,/* wItemLength: Total length of Report descriptor */
+		0x00,
+
+		/******************** Descriptor of Custom HID endpoints ******************/
+		/* 27 */
+		0x07, /* bLength: Endpoint Descriptor size */
+		USB_ENDPOINT_DESCRIPTOR_TYPE, /* bDescriptorType: */
+
+		0x81, /* bEndpointAddress: Endpoint Address (IN) */
+		0x03, /* bmAttributes: Interrupt endpoint */
+		0x02, /* wMaxPacketSize: 2 Bytes max */
+		0x00, 0x20, /* bInterval: Polling Interval (32 ms) */
+		/* 34 */
+
+		0x07, /* bLength: Endpoint Descriptor size */
+		USB_ENDPOINT_DESCRIPTOR_TYPE, /* bDescriptorType: */
+		/*	Endpoint descriptor type */
+		0x01, /* bEndpointAddress: */
+		/*	Endpoint Address (OUT) */
+		0x03, /* bmAttributes: Interrupt endpoint */
+		0x02, /* wMaxPacketSize: 2 Bytes max  */
+		0x00, 0x20, /* bInterval: Polling Interval (20 ms) */
+		/* 41 */
+	};
 
 /* Local prototypes */
 static void PIOS_USB_CB_Reset(void);
@@ -125,15 +187,27 @@ static uint8_t *PIOS_USB_CB_GetConfigDescriptor(uint16_t Length);
 static uint8_t *PIOS_USB_CB_GetStringDescriptor(uint16_t Length);
 static RESULT PIOS_USB_CB_Get_Interface_Setting(uint8_t Interface, uint8_t AlternateSetting);
 
+
 /* USB callback vectors */
 static const DEVICE My_Device_Table = {PIOS_USB_EP_NUM, 1};
 static const DEVICE_PROP My_Device_Property = {
 		0, /* PIOS_USB_CB_Init, */
-		PIOS_USB_CB_Reset, PIOS_USB_CB_Status_In, PIOS_USB_CB_Status_Out, PIOS_USB_CB_Data_Setup, PIOS_USB_CB_NoData_Setup,
-		PIOS_USB_CB_Get_Interface_Setting, PIOS_USB_CB_GetDeviceDescriptor, PIOS_USB_CB_GetConfigDescriptor, PIOS_USB_CB_GetStringDescriptor, 0, 0x40 /*MAX PACKET SIZE*/
+		PIOS_USB_CB_Reset,
+		PIOS_USB_CB_Status_In,
+		PIOS_USB_CB_Status_Out,
+		PIOS_USB_CB_Data_Setup,
+		PIOS_USB_CB_NoData_Setup,
+		PIOS_USB_CB_Get_Interface_Setting,
+		PIOS_USB_CB_GetDeviceDescriptor,
+		PIOS_USB_CB_GetConfigDescriptor,
+		PIOS_USB_CB_GetStringDescriptor,
+		0,
+		0x40 /*MAX PACKET SIZE*/
 };
-static const USER_STANDARD_REQUESTS My_User_Standard_Requests = {NOP_Process, /* PIOS_USB_CB_GetConfiguration, */
-		PIOS_USB_CB_SetConfiguration, NOP_Process, /* PIOS_USB_CB_GetInterface, */
+static const USER_STANDARD_REQUESTS My_User_Standard_Requests = {
+		NOP_Process, /* PIOS_USB_CB_GetConfiguration, */
+		PIOS_USB_CB_SetConfiguration,
+		NOP_Process, /* PIOS_USB_CB_GetInterface, */
 		NOP_Process, /* PIOS_USB_CB_SetInterface, */
 		NOP_Process, /* PIOS_USB_CB_GetStatus, */
 		NOP_Process, /* PIOS_USB_CB_ClearFeature, */
@@ -145,7 +219,7 @@ static const USER_STANDARD_REQUESTS My_User_Standard_Requests = {NOP_Process, /*
 /* USB Device informations */
 static DEVICE_INFO My_Device_Info;
 /* USB device status */
-static __IO uint32_t bDeviceState = UNCONNECTED;
+static volatile uint32_t bDeviceState = UNCONNECTED;
 
 /**
 * Initialises USB interface
@@ -170,34 +244,41 @@ int32_t PIOS_USB_Init(uint32_t mode)
 
 	/* Clear all USB interrupt requests */
 	PIOS_IRQ_Disable();
-	_SetCNTR(0); /* Interrupt Mask */
+	/* Interrupt Mask */
+	_SetCNTR(0);
 	PIOS_IRQ_Enable();
 
-	/* if mode != 2: install PIOS hooks */
-	/* a local driver can install it's own hooks and call PIOS_USB_Init(2) to force re-enumeration */
+	/* If mode != 2: install PIOS hooks */
+	/* A local driver can install it's own hooks and call PIOS_USB_Init(2) to force re-enumeration */
 	if(mode != 2) {
-		pInformation = &My_Device_Info; /* Note: usually no need to duplicate this for external drivers */
+		/* Note: usually no need to duplicate this for external drivers */
+		pInformation = &My_Device_Info;
 
 		/* Following hooks/pointers should be replaced by external drivers */
 		memcpy(&Device_Table, (DEVICE *) &My_Device_Table, sizeof(Device_Table));
 		pProperty = (DEVICE_PROP *) &My_Device_Property;
 		pUser_Standard_Requests = (USER_STANDARD_REQUESTS *) &My_User_Standard_Requests;
+
+		#ifndef DISABLE_HID
+		pEpInt_OUT[0] = PIOS_USB_HID_EP1_OUT_Callback;
+		#endif
 	}
 
+	PIOS_USB_HID_ChangeConnectionState(0);
+
 	pInformation->ControlState = 2;
 	pInformation->Current_Configuration = 0;
 
-	/* if mode == 0: don't initialise USB if not required (important for BSL) */
+	/* If mode == 0: don't initialise USB if not required (important for BSL) */
 	if(mode == 0 && PIOS_USB_IsInitialized()) {
 		pInformation->Current_Feature = PIOS_USB_ConfigDescriptor[7];
 		pInformation->Current_Configuration = 1;
 		pUser_Standard_Requests->User_SetConfiguration();
-
 	} else {
 		/* Force USB reset and power-down (this will also release the USB pins for direct GPIO control) */
 		_SetCNTR(CNTR_FRES | CNTR_PDWN);
 
-#if 0
+		#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 */
@@ -213,7 +294,7 @@ int32_t PIOS_USB_Init(uint32_t mode)
 
 		GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD;
 		GPIO_Init(USB_ACC_GPIO_PORT, &GPIO_InitStructure);
-#endif
+		#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 */
@@ -231,7 +312,6 @@ int32_t PIOS_USB_Init(uint32_t mode)
 
 		/* Release power-down, still hold reset */
 		_SetCNTR(CNTR_PDWN);
-
 		PIOS_DELAY_WaituS(5);
 
 		/* CNTR_FRES = 0 */
@@ -253,12 +333,12 @@ int32_t PIOS_USB_Init(uint32_t mode)
 		_SetISTR(0);
 
 		/* Set interrupts mask */
-		_SetCNTR(IMR_MSK); /* Interrupt mask */
+		_SetCNTR(IMR_MSK);
 	}
 
 	bDeviceState = UNCONNECTED;
 
-	/* Enable USB interrupts (unfortunately shared with CAN Rx0, as either CAN or USB can be used, but not at the same time) */
+	/* Enable USB interrupts */
 	NVIC_InitTypeDef NVIC_InitStructure;
 	NVIC_InitStructure.NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn;
 	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_USB_PRIORITY;
@@ -311,6 +391,7 @@ int32_t PIOS_USB_IsInitialized(void)
 * Hooks of STM32 USB library
 * \note Applications shouldn't call this function directly, instead please use \ref PIOS_COM layer functions
 */
+
 /**
 * Reset Routine
 */
@@ -319,14 +400,14 @@ static void PIOS_USB_CB_Reset(void)
 	/* Set PIOS Device as not configured */
 	pInformation->Current_Configuration = 0;
 
-	/* Current Feature initialization */
+	/* Current Feature initialisation */
 	pInformation->Current_Feature = PIOS_USB_ConfigDescriptor[7];
 
 	/* Set PIOS Device with the default Interface */
 	pInformation->Current_Interface = 0;
 	SetBTABLE(PIOS_USB_BTABLE_ADDRESS);
 
-	/* Initialize Endpoint 0 */
+	/* Initialise Endpoint 0 */
 	SetEPType(ENDP0, EP_CONTROL);
 	SetEPTxStatus(ENDP0, EP_TX_STALL);
 	SetEPRxAddr(ENDP0, PIOS_USB_ENDP0_RXADDR);
@@ -335,6 +416,20 @@ static void PIOS_USB_CB_Reset(void)
 	SetEPRxCount(ENDP0, pProperty->MaxPacketSize);
 	SetEPRxValid(ENDP0);
 
+	#ifndef DISABLE_HID
+	/* Initialise Endpoint 1 */
+	SetEPType(ENDP1, EP_INTERRUPT);
+	SetEPTxAddr(ENDP1, PIOS_USB_ENDP1_TXADDR);
+	SetEPRxAddr(ENDP1, PIOS_USB_ENDP1_RXADDR);
+	SetEPTxCount(ENDP1, 2);
+	SetEPRxCount(ENDP1, 2);
+	SetEPTxStatus(ENDP1, EP_TX_NAK);
+	SetEPRxStatus(ENDP1, EP_RX_VALID);
+
+	/* Propagate connection state to USB HID driver */
+	PIOS_USB_HID_ChangeConnectionState(0);
+	#endif
+
 	/* Set this device to response on default address */
 	SetDeviceAddress(0);
 
@@ -347,7 +442,10 @@ static void PIOS_USB_CB_Reset(void)
 static void PIOS_USB_CB_SetConfiguration(void)
 {
 	if(pInformation->Current_Configuration != 0) {
+		/* Propagate connection state to USB HID driver */
+		PIOS_USB_HID_ChangeConnectionState(1); /* Connected */
 		bDeviceState = CONFIGURED;
+
 	}
 }
 
@@ -380,6 +478,11 @@ static void PIOS_USB_CB_Status_Out(void)
 */
 static RESULT PIOS_USB_CB_Data_Setup(uint8_t RequestNo)
 {
+	RESULT Result;
+	if((Result = PIOS_USB_HID_CB_Data_Setup(RequestNo)) != USB_UNSUPPORT) {
+		return Result;
+	}
+
 	return USB_UNSUPPORT;
 }
 
@@ -388,6 +491,11 @@ static RESULT PIOS_USB_CB_Data_Setup(uint8_t RequestNo)
 */
 static RESULT PIOS_USB_CB_NoData_Setup(uint8_t RequestNo)
 {
+	RESULT res;
+	if((res = PIOS_USB_HID_CB_NoData_Setup(RequestNo)) != USB_UNSUPPORT) {
+		return res;
+	}
+
 	return USB_UNSUPPORT;
 }
 
@@ -423,14 +531,14 @@ static uint8_t *PIOS_USB_CB_GetStringDescriptor(uint16_t Length)
 
 	switch(pInformation->USBwValue0) {
 		case 0: /* Language */
-			/* buffer[0] and [1] initialized below */
+			/* buffer[0] and [1] initialised below */
 			buffer[2] = 0x09; // CharSet
 			buffer[3] = 0x04; // U.S.
 			len = 4;
 			break;
 
 		case 1: /* Vendor */
-			/* buffer[0] and [1] initialized below */
+			/* buffer[0] and [1] initialised below */
 			for(i = 0, len = 2; vendor_str[i] != '\0' && len < 200; ++i) {
 				buffer[len++] = vendor_str[i];
 				buffer[len++] = 0;
@@ -438,7 +546,7 @@ static uint8_t *PIOS_USB_CB_GetStringDescriptor(uint16_t Length)
 			break;
 
 		case 2: /* Product */
-			/* buffer[0] and [1] initialized below */
+			/* buffer[0] and [1] initialised below */
 			for(i = 0, len = 2; product_str[i] != '\0' && len < 200; ++i) {
 				buffer[len++] = product_str[i];
 				buffer[len++] = 0;
diff --git a/flight/PiOS/STM32F10x/pios_usb_hid.c b/flight/PiOS/STM32F10x/pios_usb_hid.c
new file mode 100644
index 000000000..ab982b576
--- /dev/null
+++ b/flight/PiOS/STM32F10x/pios_usb_hid.c
@@ -0,0 +1,305 @@
+/**
+ ******************************************************************************
+ *
+ * @file       pios_usb_hid.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * 		Parts by Thorsten Klose (tk@midibox.org)
+ * @brief      USB HID functions
+ * @see        The GNU Public License (GPL) Version 3
+ * @defgroup   PIOS_USB_HID USB HID Functions
+ * @{
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/* Project Includes */
+#include "pios.h"
+
+/* Local types */
+typedef enum _HID_REQUESTS {
+	GET_REPORT = 1,
+	GET_IDLE,
+	GET_PROTOCOL,
+	SET_REPORT = 9,
+	SET_IDLE,
+	SET_PROTOCOL
+} HID_REQUESTS;
+
+/* Local Variables */
+uint32_t ProtocolValue;
+
+/* Local Functions */
+static uint8_t *PIOS_USB_HID_GetHIDDescriptor(uint16_t Length);
+static uint8_t *PIOS_USB_HID_GetReportDescriptor(uint16_t Length);
+static uint8_t *PIOS_USB_HID_GetProtocolValue(uint16_t Length);
+
+const uint8_t PIOS_USB_HID_ReportDescriptor[PIOS_USB_HID_SIZ_REPORT_DESC] = {
+		0x05, 0x8c, /* USAGE_PAGE (ST Page)           */
+		0x09, 0x01, /* USAGE (Demo Kit)               */
+		0xa1, 0x01, /* COLLECTION (Application)       */
+		/* 6 */
+
+		/* Led 1 */
+		0x85, 0x01, /*     REPORT_ID (1)		     */
+		0x09, 0x01, /*     USAGE (LED 1)	             */
+		0x15, 0x00, /*     LOGICAL_MINIMUM (0)        */
+		0x25, 0x01, /*     LOGICAL_MAXIMUM (1)        */
+		0x75, 0x08, /*     REPORT_SIZE (8)            */
+		0x95, 0x01, /*     REPORT_COUNT (1)           */
+		0xB1, 0x82, /*    FEATURE (Data,Var,Abs,Vol) */
+
+		0x85, 0x01, /*     REPORT_ID (1)              */
+		0x09, 0x01, /*     USAGE (LED 1)              */
+		0x91, 0x82, /*     OUTPUT (Data,Var,Abs,Vol)  */
+		/* 26 */
+
+		/* Led 2 */
+		0x85, 0x02, /*     REPORT_ID 2		     */
+		0x09, 0x02, /*     USAGE (LED 2)	             */
+		0x15, 0x00, /*     LOGICAL_MINIMUM (0)        */
+		0x25, 0x01, /*     LOGICAL_MAXIMUM (1)        */
+		0x75, 0x08, /*     REPORT_SIZE (8)            */
+		0x95, 0x01, /*     REPORT_COUNT (1)           */
+		0xB1, 0x82, /*    FEATURE (Data,Var,Abs,Vol) */
+
+		0x85, 0x02, /*     REPORT_ID (2)              */
+		0x09, 0x02, /*     USAGE (LED 2)              */
+		0x91, 0x82, /*     OUTPUT (Data,Var,Abs,Vol)  */
+		/* 46 */
+
+		/* Led 3 */
+		0x85, 0x03, /*     REPORT_ID (3)		     */
+		0x09, 0x03, /*     USAGE (LED 3)	             */
+		0x15, 0x00, /*     LOGICAL_MINIMUM (0)        */
+		0x25, 0x01, /*     LOGICAL_MAXIMUM (1)        */
+		0x75, 0x08, /*     REPORT_SIZE (8)            */
+		0x95, 0x01, /*     REPORT_COUNT (1)           */
+		0xB1, 0x82, /*    FEATURE (Data,Var,Abs,Vol) */
+
+		0x85, 0x03, /*     REPORT_ID (3)              */
+		0x09, 0x03, /*     USAGE (LED 3)              */
+		0x91, 0x82, /*     OUTPUT (Data,Var,Abs,Vol)  */
+		/* 66 */
+
+		/* Led 4 */
+		0x85, 0x04, /*     REPORT_ID 4)		     */
+		0x09, 0x04, /*     USAGE (LED 4)	             */
+		0x15, 0x00, /*     LOGICAL_MINIMUM (0)        */
+		0x25, 0x01, /*     LOGICAL_MAXIMUM (1)        */
+		0x75, 0x08, /*     REPORT_SIZE (8)            */
+		0x95, 0x01, /*     REPORT_COUNT (1)           */
+		0xB1, 0x82, /*     FEATURE (Data,Var,Abs,Vol) */
+
+		0x85, 0x04, /*     REPORT_ID (4)              */
+		0x09, 0x04, /*     USAGE (LED 4)              */
+		0x91, 0x82, /*     OUTPUT (Data,Var,Abs,Vol)  */
+		/* 86 */
+
+		/* key Push Button */
+		0x85, 0x05, /*     REPORT_ID (5)              */
+		0x09, 0x05, /*     USAGE (Push Button)        */
+		0x15, 0x00, /*     LOGICAL_MINIMUM (0)        */
+		0x25, 0x01, /*     LOGICAL_MAXIMUM (1)        */
+		0x75, 0x01, /*     REPORT_SIZE (1)            */
+		0x81, 0x82, /*     INPUT (Data,Var,Abs,Vol)   */
+
+		0x09, 0x05, /*     USAGE (Push Button)        */
+		0x75, 0x01, /*     REPORT_SIZE (1)            */
+		0xb1, 0x82, /*     FEATURE (Data,Var,Abs,Vol) */
+
+		0x75, 0x07, /*     REPORT_SIZE (7)            */
+		0x81, 0x83, /*     INPUT (Cnst,Var,Abs,Vol)   */
+		0x85, 0x05, /*     REPORT_ID (2)              */
+
+		0x75, 0x07, /*     REPORT_SIZE (7)            */
+		0xb1, 0x83, /*     FEATURE (Cnst,Var,Abs,Vol) */
+		/* 114 */
+
+		/* Tamper Push Button */
+		0x85, 0x06, /*     REPORT_ID (6)              */
+		0x09, 0x06, /*     USAGE (Tamper Push Button) */
+		0x15, 0x00, /*     LOGICAL_MINIMUM (0)        */
+		0x25, 0x01, /*     LOGICAL_MAXIMUM (1)        */
+		0x75, 0x01, /*     REPORT_SIZE (1)            */
+		0x81, 0x82, /*     INPUT (Data,Var,Abs,Vol)   */
+
+		0x09, 0x06, /*     USAGE (Tamper Push Button) */
+		0x75, 0x01, /*     REPORT_SIZE (1)            */
+		0xb1, 0x82, /*     FEATURE (Data,Var,Abs,Vol) */
+
+		0x75, 0x07, /*     REPORT_SIZE (7)            */
+		0x81, 0x83, /*     INPUT (Cnst,Var,Abs,Vol)   */
+		0x85, 0x06, /*     REPORT_ID (6)              */
+
+		0x75, 0x07, /*     REPORT_SIZE (7)            */
+		0xb1, 0x83, /*     FEATURE (Cnst,Var,Abs,Vol) */
+		/* 142 */
+
+		/* ADC IN */
+		0x85, 0x07, /*     REPORT_ID (7)              */
+		0x09, 0x07, /*     USAGE (ADC IN)             */
+		0x15, 0x00, /*     LOGICAL_MINIMUM (0)        */
+		0x26, 0xff, 0x00, /*     LOGICAL_MAXIMUM (255)      */
+		0x75, 0x08, /*     REPORT_SIZE (8)            */
+		0x81, 0x82, /*     INPUT (Data,Var,Abs,Vol)   */
+		0x85, 0x07, /*     REPORT_ID (7)              */
+		0x09, 0x07, /*     USAGE (ADC in)             */
+		0xb1, 0x82, /*     FEATURE (Data,Var,Abs,Vol) */
+		/* 161 */
+
+		0xc0 /*     END_COLLECTION	             */
+		};
+ONE_DESCRIPTOR PIOS_USB_HID_Report_Descriptor = {(uint8_t *) PIOS_USB_HID_ReportDescriptor, PIOS_USB_HID_SIZ_REPORT_DESC};
+ONE_DESCRIPTOR PIOS_USB_HID_Hid_Descriptor = {(uint8_t*) PIOS_USB_HID_ReportDescriptor + PIOS_USB_HID_OFF_HID_DESC, PIOS_USB_HID_SIZ_HID_DESC};
+
+/**
+* This function is called by the USB driver on cable connection/disconnection
+* \param[in] connected connection status (1 if connected)
+* \return < 0 on errors
+* \note Applications shouldn't call this function directly, instead please use \ref PIOS_COM layer functions
+*/
+uint32_t PIOS_USB_HID_ChangeConnectionState(uint32_t Connected)
+{
+	return 0;
+}
+
+int32_t PIOS_USB_HID_CB_Data_Setup(uint8_t RequestNo)
+{
+	uint8_t *(*CopyRoutine)( uint16_t) = NULL;
+
+	CopyRoutine = NULL;
+
+	if((RequestNo == GET_DESCRIPTOR) && (Type_Recipient == (STANDARD_REQUEST | INTERFACE_RECIPIENT)) && (pInformation->USBwIndex0 == 0)) {
+
+		if(pInformation->USBwValue1 == PIOS_USB_HID_REPORT_DESCRIPTOR) {
+			CopyRoutine = PIOS_USB_HID_GetReportDescriptor;
+		} else if(pInformation->USBwValue1 == PIOS_USB_HID_HID_DESCRIPTOR_TYPE) {
+			CopyRoutine = PIOS_USB_HID_GetHIDDescriptor;
+		}
+
+	}
+	/* End of GET_DESCRIPTOR */
+
+	/* GET_PROTOCOL */
+	else if((Type_Recipient == (CLASS_REQUEST | INTERFACE_RECIPIENT)) && RequestNo == GET_PROTOCOL) {
+		CopyRoutine = PIOS_USB_HID_GetProtocolValue;
+	}
+
+	if(CopyRoutine == NULL) {
+		return USB_UNSUPPORT;
+	}
+
+	pInformation->Ctrl_Info.CopyData = CopyRoutine;
+	pInformation->Ctrl_Info.Usb_wOffset = 0;
+	(*CopyRoutine)(0);
+
+	return USB_SUCCESS;
+}
+
+int32_t PIOS_USB_HID_CB_NoData_Setup(uint8_t RequestNo)
+{
+	if((Type_Recipient == (CLASS_REQUEST | INTERFACE_RECIPIENT)) && (RequestNo == SET_PROTOCOL)) {
+		uint8_t wValue0 = pInformation->USBwValue0;
+		ProtocolValue = wValue0;
+		return USB_SUCCESS;
+	}
+
+	else {
+		return USB_UNSUPPORT;
+	}
+
+	// TODO:Unsure
+	return USB_UNSUPPORT;
+}
+
+/**
+* Gets the HID descriptor.
+* \param[in] Length
+* \return The address of the configuration descriptor.
+*/
+static uint8_t *PIOS_USB_HID_GetHIDDescriptor(uint16_t Length)
+{
+	return Standard_GetDescriptorData(Length, &PIOS_USB_HID_Hid_Descriptor);
+}
+
+/**
+* Gets the HID report descriptor.
+* \param[in] Length
+* \return The address of the configuration descriptor.
+*/
+static uint8_t *PIOS_USB_HID_GetReportDescriptor(uint16_t Length)
+{
+	return Standard_GetDescriptorData(Length, &PIOS_USB_HID_Report_Descriptor);
+}
+
+/**
+* Gets the protocol value
+* \param[in] Length
+* \return address of the protcol value.
+*/
+static uint8_t *PIOS_USB_HID_GetProtocolValue(uint16_t Length)
+{
+	if(Length == 0) {
+		pInformation->Ctrl_Info.Usb_wLength = 1;
+		return NULL;
+	} else {
+		return (uint8_t *) (&ProtocolValue);
+	}
+}
+
+/**
+* EP1 OUT Callback Routine
+*/
+void PIOS_USB_HID_EP1_OUT_Callback(void)
+{
+	uint8_t Receive_Buffer[2];
+	BitAction Led_State;
+
+	/* Read received data (2 bytes) */
+	USB_SIL_Read(0x01, Receive_Buffer);
+
+	if(Receive_Buffer[1] == 0) {
+		Led_State = Bit_RESET;
+	} else {
+		Led_State = Bit_SET;
+	}
+
+	switch(Receive_Buffer[0]) {
+		case 1: /* Led 1 */
+			if(Led_State != Bit_RESET) {
+				PIOS_LED_On(LED1);
+			} else {
+				PIOS_LED_Off(LED1);
+			}
+			break;
+		case 2: /* Led 2 */
+			if(Led_State != Bit_RESET) {
+				PIOS_LED_On(LED2);
+			} else {
+				PIOS_LED_Off(LED2);
+			}
+			break;
+		default:
+
+			PIOS_LED_Off(LED1);
+			PIOS_LED_Off(LED2);
+			break;
+	}
+
+	SetEPRxStatus(ENDP1, EP_RX_VALID);
+}
+
diff --git a/flight/PiOS/inc/pios_usb.h b/flight/PiOS/inc/pios_usb.h
index c2df72ea3..d1a474de8 100644
--- a/flight/PiOS/inc/pios_usb.h
+++ b/flight/PiOS/inc/pios_usb.h
@@ -30,43 +30,38 @@
 /* Local defines */
 /* Following settings allow to customise the USB device descriptor */
 #ifndef PIOS_USB_VENDOR_ID
-#define PIOS_USB_VENDOR_ID    0x16c0        // sponsored by voti.nl! see http://www.voti.nl/pids
+#define PIOS_USB_VENDOR_ID		0x0483
 #endif
 
 #ifndef PIOS_USB_VENDOR_STR
-#define PIOS_USB_VENDOR_STR   "openpilot.org" // you will see this in the USB device description
+#define PIOS_USB_VENDOR_STR		"openpilot.org"
 #endif
 
 #ifndef PIOS_USB_PRODUCT_STR
-#define PIOS_USB_PRODUCT_STR  "OpenPilot"      /* You will see this in the USB device list */
+#define PIOS_USB_PRODUCT_STR		"OpenPilot"
 #endif
 
 #ifndef PIOS_USB_PRODUCT_ID
-#define PIOS_USB_PRODUCT_ID   0x03ff        /* ==1023; 1020-1029 reserved for T.Klose, 1000 - 1009 free for lab use */
+#define PIOS_USB_PRODUCT_ID		0x5750
 #endif
 
 #ifndef PIOS_USB_VERSION_ID
-#define PIOS_USB_VERSION_ID   0x0100        /* v1.00 */
+#define PIOS_USB_VERSION_ID		0x0200        /* v2.00 */
 #endif
 
 /* Internal defines which are used by PIOS USB HID (don't touch) */
-#define PIOS_USB_EP_NUM   5
+#define PIOS_USB_EP_NUM			2
 
 /* Buffer table base address */
-#define PIOS_USB_BTABLE_ADDRESS      0x000
+#define PIOS_USB_BTABLE_ADDRESS		0x000
 
 /* EP0 rx/tx buffer base address */
-#define PIOS_USB_ENDP0_RXADDR        0x040
-#define PIOS_USB_ENDP0_TXADDR        0x080
+#define PIOS_USB_ENDP0_RXADDR		0x040
+#define PIOS_USB_ENDP0_TXADDR		0x080
 
 /* EP1 Rx/Tx buffer base address for HID driver */
-#define PIOS_USB_ENDP1_TXADDR        0x0c0
-#define PIOS_USB_ENDP1_RXADDR        0x100
-
-/* EP2/3/4 buffer base addresses for COM driver */
-#define PIOS_USB_ENDP2_TXADDR        0x140
-#define PIOS_USB_ENDP3_RXADDR        0x180
-#define PIOS_USB_ENDP4_TXADDR        0x1c0
+#define PIOS_USB_ENDP1_TXADDR		0x0C0
+#define PIOS_USB_ENDP1_RXADDR		0x100
 
 
 /* Global Variables */
diff --git a/flight/PiOS/inc/pios_usb_hid.h b/flight/PiOS/inc/pios_usb_hid.h
new file mode 100644
index 000000000..55d33058f
--- /dev/null
+++ b/flight/PiOS/inc/pios_usb_hid.h
@@ -0,0 +1,46 @@
+/**
+ ******************************************************************************
+ *
+ * @file       pios_usb_hid.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * 	        Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org)
+ * @brief      USB HID layer functions header
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef PIOS_USB_HID_H
+#define PIOS_USB_HID_H
+
+/* Global Definitions */
+#define PIOS_USB_HID_SIZ_REPORT_DESC		162
+#define PIOS_USB_HID_REPORT_DESCRIPTOR		0x22
+#define PIOS_USB_HID_HID_DESCRIPTOR_TYPE	0x21
+#define PIOS_USB_HID_OFF_HID_DESC		0x12
+#define PIOS_USB_HID_SIZ_HID_DESC		0x09
+
+/* Global Variables */
+extern uint32_t ProtocolValue;
+
+/* Global functions */
+extern uint32_t PIOS_USB_HID_ChangeConnectionState(uint32_t Connected);
+extern int32_t PIOS_USB_HID_CB_Data_Setup(uint8_t RequestNo);
+extern int32_t PIOS_USB_HID_CB_NoData_Setup(uint8_t RequestNo);
+extern void PIOS_USB_HID_EP1_OUT_Callback(void);
+
+#endif /* PIOS_USB_HID_H */
diff --git a/flight/PiOS/pios.c b/flight/PiOS/pios.c
index 69178dc86..532a0038a 100644
--- a/flight/PiOS/pios.c
+++ b/flight/PiOS/pios.c
@@ -44,10 +44,11 @@ static uint8_t sdcard_available;
 
 /* Function Prototypes */
 static void TaskTick(void *pvParameters);
+static void TaskServos(void *pvParameters);
 static void TaskHooks(void *pvParameters);
+static void TaskSDCard(void *pvParameters);
 int32_t CONSOLE_Parse(COMPortTypeDef port, char c);
 void OP_ADC_NotifyChange(uint32_t pin, uint32_t pin_value);
-static void TaskSDCard(void *pvParameters);
 
 /**
 * Main function
@@ -100,15 +101,37 @@ int main()
 
 	PIOS_USB_Init(0);
 
-	PIOS_COM_ReceiveCallbackInit(CONSOLE_Parse);
+	__IO uint8_t Send_Buffer[2];
+	uint8_t count = 0;
+
+	for(;;)
+	{
+		/* Report ID */
+		Send_Buffer[0] = 0x07;
+
+		/* Report Data */
+		Send_Buffer[1] = count;
+		USB_SIL_Write(EP1_IN, (uint8_t*) Send_Buffer, 2);
+
+		if(count >= 255) {
+			count = 0;
+		} else {
+			count++;
+		}
+
+		PIOS_DELAY_WaitmS(50);
+	}
+
+	//PIOS_COM_ReceiveCallbackInit(CONSOLE_Parse);
 
 	/* Initialise OpenPilot application */
 //	OpenPilotInit();
 
 	/* Create a FreeRTOS task */
-	//xTaskCreate(TaskTick, (signed portCHAR *)"Test", configMINIMAL_STACK_SIZE , NULL, 1, NULL);
+	xTaskCreate(TaskTick, (signed portCHAR *)"Test", configMINIMAL_STACK_SIZE , NULL, 1, 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);
+	//xTaskCreate(TaskSDCard, (signed portCHAR *)"SDCard", configMINIMAL_STACK_SIZE, NULL, (tskIDLE_PRIORITY + 2), NULL);
 
 	/* Start the FreeRTOS scheduler */
 	vTaskStartScheduler();
@@ -149,7 +172,7 @@ void OP_ADC_NotifyChange(uint32_t pin, uint32_t pin_value)
 
 }
 
-void TaskTick(void *pvParameters)
+static void TaskTick(void *pvParameters)
 {
 	portTickType xLastExecutionTime;
 
@@ -162,13 +185,51 @@ void TaskTick(void *pvParameters)
 		PIOS_LED_Toggle(LED1);
 		vTaskDelayUntil(&xLastExecutionTime, 500 / portTICK_RATE_MS);
 	}
+}
 
-#if 0
+static void TaskServos(void *pvParameters)
+{
 	/* For testing servo outputs */
-	const portTickType xDelay = 1 / portTICK_RATE_MS;
+	portTickType xDelay;
 
-	Used to test servos, cycles all servos from one side to the other
+	/* Used to test servos, cycles all servos from one side to the other */
 	for(;;) {
+		xDelay = 250 / portTICK_RATE_MS;
+		PIOS_Servo_Set(0, 2000);
+		vTaskDelay(xDelay);
+		PIOS_Servo_Set(1, 2000);
+		vTaskDelay(xDelay);
+		PIOS_Servo_Set(2, 2000);
+		vTaskDelay(xDelay);
+		PIOS_Servo_Set(3, 2000);
+		vTaskDelay(xDelay);
+		PIOS_Servo_Set(4, 2000);
+		vTaskDelay(xDelay);
+		PIOS_Servo_Set(5, 2000);
+		vTaskDelay(xDelay);
+		PIOS_Servo_Set(6, 2000);
+		vTaskDelay(xDelay);
+		PIOS_Servo_Set(7, 2000);
+		vTaskDelay(xDelay);
+
+		PIOS_Servo_Set(7, 1000);
+		vTaskDelay(xDelay);
+		PIOS_Servo_Set(6, 1000);
+		vTaskDelay(xDelay);
+		PIOS_Servo_Set(5, 1000);
+		vTaskDelay(xDelay);
+		PIOS_Servo_Set(4, 1000);
+		vTaskDelay(xDelay);
+		PIOS_Servo_Set(3, 1000);
+		vTaskDelay(xDelay);
+		PIOS_Servo_Set(2, 1000);
+		vTaskDelay(xDelay);
+		PIOS_Servo_Set(1, 1000);
+		vTaskDelay(xDelay);
+		PIOS_Servo_Set(0, 1000);
+		vTaskDelay(xDelay);
+
+		xDelay = 1 / portTICK_RATE_MS;
 		for(int i = 1000; i < 2000; i++) {
 			PIOS_Servo_Set(0, i);
 			PIOS_Servo_Set(1, i);
@@ -192,7 +253,6 @@ void TaskTick(void *pvParameters)
 			vTaskDelay(xDelay);
 		}
 	}
-#endif
 }
 
 static void TaskHooks(void *pvParameters)
diff --git a/flight/PiOS/pios.h b/flight/PiOS/pios.h
index 321eb158e..84cd36bc9 100644
--- a/flight/PiOS/pios.h
+++ b/flight/PiOS/pios.h
@@ -66,6 +66,7 @@
 #include <pios_spi.h>
 #include <pios_pwm.h>
 #include <pios_usb.h>
+#include <pios_usb_hid.h>
 
 /* PIOS Hardware Includes (Common) */
 #include <pios_settings.h>
@@ -75,5 +76,7 @@
 
 /* More added here as they get written */
 
+/* USB Libs */
+#include <usb_lib.h>
 
 #endif /* PIOS_H */