diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino
index 6544cc7..c96a190 100644
--- a/Multiprotocol/Telemetry.ino
+++ b/Multiprotocol/Telemetry.ino
@@ -26,9 +26,23 @@
 uint8_t pktx[MAX_PKTX];
 uint8_t pktx1[MAX_PKTX];//second buffer for sport
 uint8_t idxt;
-uint8_t pass = 0;
 uint8_t frame[18];
 
+
+#ifdef BASH_SERIAL
+// For bit-bashed serial output
+
+struct t_serial_bash
+{
+	uint8_t head ;
+	uint8_t tail ;
+	uint8_t data[64] ;
+	uint8_t busy ;
+	uint8_t speed ;
+} SerialControl ;
+#endif
+
+
 #if defined DSM_TELEMETRY
 void DSM2_frame()
 {
@@ -422,6 +436,7 @@ void TelemetryUpdate()
 		return ;
 	}
         #endif
+        
 	#if defined DSM_TELEMETRY
 	if(telemetry_link && (cur_protocol[0]&0x1F) == MODE_DSM2 )
 	{	// DSM2
@@ -449,7 +464,26 @@ void TelemetryUpdate()
 	
 }
 
+
+/**************************/
+/**************************/
+/**  Serial TX routines  **/
+/**************************/
+/**************************/
+
+
 #ifndef BASH_SERIAL
+// Routines for normal serial output
+void Serial_write(uint8_t data)
+{
+	uint8_t nextHead ;
+	nextHead = tx_head + 1 ;
+	if ( nextHead >= TXBUFFER_SIZE )
+		nextHead = 0 ;
+	tx_buff[nextHead]=data;
+	tx_head = nextHead ;
+	tx_resume();
+}
 
 // Speed is 0 for 100K and 1 for 9600
 void initTXSerial( uint8_t speed)
@@ -468,6 +502,7 @@ void initTXSerial( uint8_t speed)
 	#ifdef STM32_board
                 Serial2.begin(9600);//USART3 
 		USART3_BASE->CR1 &= ~ USART_CR1_RE;//disable RX leave TX enabled
+        }
 	#else
 		//9600 bauds
 		UBRR0H = 0x00;
@@ -479,7 +514,7 @@ void initTXSerial( uint8_t speed)
 	UCSR0B |= (1<<TXEN0);//tx enable
 	#endif
 	#endif
-	#endif
+
 }
 
 
@@ -534,52 +569,6 @@ void initTXSerial( uint8_t speed)
 	#endif	
 #endif
 
-
-	void Serial_write(uint8_t data)
-	{
-		cli();	// disable global int
-		if(++tx_head>=TXBUFFER_SIZE)
-		tx_head=0;
-		tx_buff[tx_head]=data;
-		#ifdef XMEGA
-			USARTC0.CTRLA = (USARTC0.CTRLA & 0xFC) | 0x01 ;
-			#else
-			#if defined STM32_board
-			USART3_BASE->CR1 |= USART_CR1_TXEIE;
-				#else
-				UCSR0B |= (1<<UDRIE0);//enable UDRE interrupt
-			#endif
-		#endif
-		sei();	// enable global int
-	}
-	
-
-	static void PPM_Telemetry_serial_init()
-	{
-		#ifdef XMEGA
-			USARTC0.BAUDCTRLA = 207 ;
-			USARTC0.BAUDCTRLB = 0 ;
-			
-			USARTC0.CTRLB = 0x18 ;
-			USARTC0.CTRLA = (USARTC0.CTRLA & 0xCF) | 0x10 ;
-			USARTC0.CTRLC = 0x03 ;
-			#else
-			#if defined STM32_board
-				Serial1.begin(9600);				
-				#else
-				//9600 bauds
-				UBRR0H = 0x00;
-				UBRR0L = 0x67;
-				UCSR0A = 0 ;	// Clear X2 bit
-				//Set frame format to 8 data bits, none, 1 stop bit
-				UCSR0C = (1<<UCSZ01)|(1<<UCSZ00);
-				UCSR0B = (1<<TXEN0);//tx enable
-			#endif	
-		#endif
-	}
-
-
-
 #endif
 
 #ifdef BASH_SERIAL