diff --git a/flight/pios/stm32f4xx/libraries/STM32_USB_Device_Library/Core/src/usbd_req.c b/flight/pios/stm32f4xx/libraries/STM32_USB_Device_Library/Core/src/usbd_req.c
index 75d038034..bdd357f94 100644
--- a/flight/pios/stm32f4xx/libraries/STM32_USB_Device_Library/Core/src/usbd_req.c
+++ b/flight/pios/stm32f4xx/libraries/STM32_USB_Device_Library/Core/src/usbd_req.c
@@ -798,7 +798,7 @@ void USBD_ParseSetupRequest( USB_OTG_CORE_HANDLE  *pdev,
 */
 
 void USBD_CtlError( USB_OTG_CORE_HANDLE  *pdev,
-                            USB_SETUP_REQ *req)
+        __attribute__((unused)) USB_SETUP_REQ *req)
 {
   
     DCD_EP_Stall(pdev , 0x80);
diff --git a/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_dcd_int.c b/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_dcd_int.c
index 0b14a3bb6..e54cf2bf6 100644
--- a/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_dcd_int.c
+++ b/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_dcd_int.c
@@ -426,7 +426,7 @@ static uint32_t DCD_HandleInEP_ISR(USB_OTG_CORE_HANDLE *pdev)
   
   uint32_t ep_intr;
   uint32_t epnum = 0;
-  uint32_t fifoemptymsk;
+//  uint32_t fifoemptymsk;
   diepint.d32 = 0;
   ep_intr = USB_OTG_ReadDevAllInEPItr(pdev);
   
@@ -437,8 +437,10 @@ static uint32_t DCD_HandleInEP_ISR(USB_OTG_CORE_HANDLE *pdev)
       diepint.d32 = DCD_ReadDevInEP(pdev , epnum); /* Get In ITR status */
       if ( diepint.b.xfercompl )
       {
+#if 0 // THIS CODE IS REMOVED, done in DCD_WriteEmptyTxFifo()
         fifoemptymsk = 0x1 << epnum;
         USB_OTG_MODIFY_REG32(&pdev->regs.DREGS->DIEPEMPMSK, fifoemptymsk, 0);
+#endif
         CLEAR_IN_EP_INTR(epnum, xfercompl);
         /* TX COMPLETE */
         USBD_DCD_INT_fops->DataInStage(pdev , epnum);
@@ -677,7 +679,15 @@ static uint32_t DCD_WriteEmptyTxFifo(USB_OTG_CORE_HANDLE *pdev, uint32_t epnum)
     
     txstatus.d32 = USB_OTG_READ_REG32(&pdev->regs.INEP_REGS[epnum]->DTXFSTS);
   }
-
+  uint32_t fifoemptymsk;
+  // --- start fix (just before return 1)
+    if (len < ep->maxpacket)
+    {
+      // FIFO empty
+      fifoemptymsk = 0x1 << epnum;
+      USB_OTG_MODIFY_REG32(&pdev->regs.DREGS->DIEPEMPMSK, fifoemptymsk, 0);
+    }
+  // end of fix
   return 1;
 }