diff --git a/radio/src/cli.cpp b/radio/src/cli.cpp
index f41bb4bff..9a39e4466 100644
--- a/radio/src/cli.cpp
+++ b/radio/src/cli.cpp
@@ -41,7 +41,6 @@
 #define CLI_COMMAND_MAX_ARGS           8
 #define CLI_COMMAND_MAX_LEN            256
 
-extern Fifo<512> uart3TxFifo;
 OS_TID cliTaskId;
 OS_STK cliStack[CLI_STACK_SIZE];
 Fifo<256> cliRxFifo;
diff --git a/radio/src/debug.cpp b/radio/src/debug.cpp
index dfb7d34aa..f210ef7d0 100644
--- a/radio/src/debug.cpp
+++ b/radio/src/debug.cpp
@@ -61,7 +61,6 @@ void debugPrintf(const char * format, ...)
 #if defined(DEBUG_TRACE_BUFFER)
 static struct TraceElement traceBuffer[TRACE_BUFFER_LEN];
 static uint8_t traceBufferPos;
-extern Fifo<512> uart3TxFifo;
 gtime_t filltm(gtime_t *t, struct gtm *tp);
 
 void trace_event(enum TraceEvent event, uint32_t data)
@@ -107,7 +106,7 @@ void dumpTraceBuffer()
     TRACE("  %03d    0x%08x", traceBuffer[n].event, traceBuffer[n].data);  
     if (traceBuffer[n].time == 0 && traceBuffer[n].time_ms == 0) break;
     if ((n % 5) == 0) {
-      while (!uart3TxFifo.empty()) { 
+      while (!serial2TxFifo.empty()) { 
         CoTickDelay(1);
       }
     }
diff --git a/radio/src/fifo.h b/radio/src/fifo.h
index dae8032fa..18a1d478a 100644
--- a/radio/src/fifo.h
+++ b/radio/src/fifo.h
@@ -56,7 +56,7 @@ class Fifo
     }
 
     bool pop(uint8_t & byte) {
-      if (empty()) {
+      if (isEmpty()) {
         return false;
       }
       else {
@@ -66,12 +66,17 @@ class Fifo
       }
     }
 
-    bool empty() {
+    bool isEmpty() {
       return (ridx == widx);
     }
 
+    bool isFull() {
+      uint32_t next = (widx+1) & (N-1);
+      return (next == ridx);
+    }
+
     void flush() {
-      while (!empty()) {};
+      while (!isEmpty()) {};
     }
 
   protected:
@@ -80,4 +85,4 @@ class Fifo
     volatile uint32_t ridx;
 };
 
-#endif
+#endif // _FIFO_H_
diff --git a/radio/src/targets/taranis/serial2_driver.cpp b/radio/src/targets/taranis/serial2_driver.cpp
index 273c09873..5b4f60036 100644
--- a/radio/src/targets/taranis/serial2_driver.cpp
+++ b/radio/src/targets/taranis/serial2_driver.cpp
@@ -37,7 +37,7 @@
 #include "../../opentx.h"
 
 uint8_t serial2Mode = 0;
-Fifo<512> uart3TxFifo;
+Fifo<512> serial2TxFifo;
 extern Fifo<512> telemetryFifo;
 extern Fifo<32> sbusFifo;
 
@@ -98,7 +98,8 @@ void serial2Init(unsigned int mode, unsigned int protocol)
 
 void serial2Putc(char c)
 {
-  uart3TxFifo.push(c);
+  while (serial2TxFifo.isFull());
+  serial2TxFifo.push(c);
   USART_ITConfig(SERIAL_USART, USART_IT_TXE, ENABLE);
 }
 
@@ -124,7 +125,7 @@ extern "C" void SERIAL_USART_IRQHandler(void)
   // Send
   if (USART_GetITStatus(SERIAL_USART, USART_IT_TXE) != RESET) {
     uint8_t txchar;
-    if (uart3TxFifo.pop(txchar)) {
+    if (serial2TxFifo.pop(txchar)) {
       /* Write one byte to the transmit data register */
       USART_SendData(SERIAL_USART, txchar);
     }
diff --git a/radio/src/targets/taranis/usbd_cdc.cpp b/radio/src/targets/taranis/usbd_cdc.cpp
index 7e742b7cd..ef4543eec 100644
--- a/radio/src/targets/taranis/usbd_cdc.cpp
+++ b/radio/src/targets/taranis/usbd_cdc.cpp
@@ -50,6 +50,7 @@ extern uint8_t  APP_Rx_Buffer []; /* Write CDC received data in this buffer.
 extern uint32_t APP_Rx_ptr_in;    /* Increment this pointer or roll it back to
                                      start address when writing received data
                                      in the buffer APP_Rx_Buffer. */
+extern uint32_t APP_Rx_ptr_out;
 
 /* Private function prototypes -----------------------------------------------*/
 static uint16_t VCP_Init     (void);
@@ -161,16 +162,23 @@ void usbSerialPutc(uint8_t c)
 {
   if (!cdcConnected) return;
 
+  uint32_t APP_Rx_length;
+  do {
+    APP_Rx_length = APP_RX_DATA_SIZE + APP_Rx_ptr_in - APP_Rx_ptr_out;
+    if (APP_Rx_length >= APP_RX_DATA_SIZE) {
+      APP_Rx_length -= APP_RX_DATA_SIZE;
+    }
+  } while (APP_Rx_length == APP_RX_DATA_SIZE-1);
+
   APP_Rx_Buffer[APP_Rx_ptr_in] = c;
   ++charsWritten;
   /* To avoid buffer overflow */
-  if(APP_Rx_ptr_in >= APP_RX_DATA_SIZE-1)
-  {
+  if (APP_Rx_ptr_in >= APP_RX_DATA_SIZE-1) {
     APP_Rx_ptr_in = 0;
     ++usbWraps;
   }
   else {
-    APP_Rx_ptr_in++;  
+    APP_Rx_ptr_in++;
   }  
 }
 
diff --git a/radio/src/targets/taranis/usbd_conf.h b/radio/src/targets/taranis/usbd_conf.h
index 530e4c103..557973a26 100644
--- a/radio/src/targets/taranis/usbd_conf.h
+++ b/radio/src/targets/taranis/usbd_conf.h
@@ -61,7 +61,7 @@
 #define CDC_CMD_PACKET_SZE           8    /* Control Endpoint Packet size */
 
 #define CDC_IN_FRAME_INTERVAL        5    /* Number of frames between IN transfers */
-#define APP_RX_DATA_SIZE             2048 // USB serial port output buffer. TODO: tune this buffer size /* Total size of IN buffer: APP_RX_DATA_SIZE*8/MAX_BAUDARATE*1000 should be > CDC_IN_FRAME_INTERVAL */
+#define APP_RX_DATA_SIZE             512 // USB serial port output buffer. TODO: tune this buffer size /* Total size of IN buffer: APP_RX_DATA_SIZE*8/MAX_BAUDARATE*1000 should be > CDC_IN_FRAME_INTERVAL */
 #define APP_FOPS                     VCP_fops
 
 #endif //__USBD_CONF__H__