diff --git a/Multiprotocol/MultiOrange.cpp.xmega b/Multiprotocol/MultiOrange.cpp.xmega
index 2a331fa..85a7ca0 100644
--- a/Multiprotocol/MultiOrange.cpp.xmega
+++ b/Multiprotocol/MultiOrange.cpp.xmega
@@ -17,7 +17,8 @@ static void module_reset(void) ;
 static void update_led_status(void) ;
 static void set_rx_tx_addr(uint32_t id) ;
 uint16_t limit_channel_100(uint8_t ch) ;
-
+void initTXSerial( uint8_t speed);
+void Serial_write(uint8_t data);
 
 extern void NRF24L01_Reset(void ) ;
 extern void A7105_Reset(void ) ;
diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino
index 0ded1cb..ec6bdc8 100644
--- a/Multiprotocol/Multiprotocol.ino
+++ b/Multiprotocol/Multiprotocol.ino
@@ -628,11 +628,19 @@ void update_serial_data()
 	RX_FLAG_off;								//data has been processed
 	do
 	{
-		cli();
+		#ifdef XMEGA
+			cli();
+		#else
+			UCSR0B &= ~(1<<RXCIE0);	// RX interrupt disable
+		#endif
 		if(IS_RX_MISSED_BUFF_on)	// If the buffer is still valid
 			memcpy((void*)rx_ok_buff,(const void*)rx_buff,RXBUFFER_SIZE);// Duplicate the buffer
-		sei();
 		RX_MISSED_BUFF_off;
+		#ifdef XMEGA
+			sei();
+		#else
+			UCSR0B |= (1<<RXCIE0) ;	// RX interrupt enable
+		#endif
 		RX_DONOTUPDTAE_on;
 		if(rx_ok_buff[0]&0x20)		//check range
 			RANGE_FLAG_on;
@@ -787,6 +795,9 @@ void Mprotocol_serial_init()
 	USARTC0.CTRLA = (USARTC0.CTRLA & 0xCF) | 0x10 ;
 	USARTC0.CTRLC = 0x2B ;
 	USARTC0.DATA ;
+	#ifdef INVERT_TELEMETRY
+		PORTC.PIN3CTRL |= 0x40 ;
+	#endif
 #else
 	#include <util/setbaud.h>	
 	UBRR0H = UBRRH_VALUE;
@@ -898,6 +909,7 @@ uint8_t SPI_Read(void)
 // replacement millis() and micros()
 // These work polled, no interrupts
 // micros() MUST be called at least once every 32 milliseconds
+#ifndef XMEGA
 uint16_t MillisPrecount ;
 uint16_t lastTimerValue ;
 uint32_t TotalMicros ;
@@ -997,6 +1009,7 @@ void init()
    // this needs to be called before setup() or some functions won't work there
    sei();
 }
+#endif //XMEGA
 
 /**************************/
 /**************************/
@@ -1053,7 +1066,7 @@ ISR(USART_RX_vect)
 	if((USARTC0.STATUS & 0x1C)==0)	// Check frame error, data overrun and parity error
 #else
 	
-	UCSR0B &= ~(1<<RXCIE0) ;		//rx interrupt disable
+	UCSR0B &= ~(1<<RXCIE0) ;		// RX interrupt disable
 	sei() ;
 
 	if((UCSR0A&0x1C)==0)			// Check frame error, data overrun and parity error
@@ -1118,7 +1131,7 @@ ISR(USART_RX_vect)
 
 #ifndef XMEGA
 	cli() ;
-	UCSR0B |= (1<<RXCIE0) ;	// RX enable interrupt
+	UCSR0B |= (1<<RXCIE0) ;	// RX interrupt enable
 #endif
 }
 
diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino
index 78d2673..0ebb062 100644
--- a/Multiprotocol/Telemetry.ino
+++ b/Multiprotocol/Telemetry.ino
@@ -497,16 +497,17 @@ void frskyUpdate()
 // Routines for normal serial output
 void Serial_write(uint8_t data)
 {
-	cli();	// disable global int
-	if(++tx_head>=TXBUFFER_SIZE)
-		tx_head=0;
-	tx_buff[tx_head]=data;
+	uint8_t nextHead ;
+	nextHead = tx_head + 1 ;
+	if ( nextHead >= TXBUFFER_SIZE )
+		nextHead = 0 ;
+	tx_buff[nextHead]=data;
+	tx_head = nextHead ;
 #ifdef XMEGA
 	USARTC0.CTRLA = (USARTC0.CTRLA & 0xFC) | 0x01 ;
 #else	
 	UCSR0B |= (1<<UDRIE0);//enable UDRE interrupt
 #endif
-	sei();	// enable global int
 }
 
 // Speed is 0 for 100K and 1 for 9600
@@ -521,6 +522,7 @@ void initTXSerial( uint8_t speed)
 		USARTC0.CTRLB = 0x18 ;
 		USARTC0.CTRLA = (USARTC0.CTRLA & 0xCF) | 0x10 ;
 		USARTC0.CTRLC = 0x03 ;
+	}
 	#else
 		//9600 bauds
 		UBRR0H = 0x00;
@@ -528,11 +530,9 @@ void initTXSerial( uint8_t speed)
 		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
 	}
-	else
-		UCSR0B |= (1<<TXEN0);//tx enable
+	UCSR0B |= (1<<TXEN0);//tx enable
+	#endif
 }
 
 #ifdef XMEGA