diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h
index 8fa151c..c951ed9 100644
--- a/Multiprotocol/Multiprotocol.h
+++ b/Multiprotocol/Multiprotocol.h
@@ -333,7 +333,7 @@ enum FailSafeMode {
 
 //Status messages
 #if defined(STM32_BOARD) && defined (SERIAL_DEBUG)
-	#define debug(msg, ...)  {char buf[64]; sprintf(buf, msg "\r\n", ##__VA_ARGS__); for(int i=0;buf[i] !=0; i++) StatusSerial_write(buf[i]);}
+	#define debug(msg, ...)  {char buf[64]; sprintf(buf, msg "\r\n", ##__VA_ARGS__); Serial1.write(buf);}
 #else
 	#define debug(...)
 	#undef SERIAL_DEBUG
diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino
index 0703c71..2bd6e90 100644
--- a/Multiprotocol/Multiprotocol.ino
+++ b/Multiprotocol/Multiprotocol.ino
@@ -61,9 +61,6 @@
 	void ISR_COMPB();
 	extern "C"
 	{
-		#ifdef SERIAL_DEBUG
-			void __irq_usart1(void);
-		#endif
 		void __irq_usart2(void);
 		void __irq_usart3(void);
 	}
@@ -190,11 +187,6 @@ uint8_t pkt[MAX_PKT];//telemetry receiving packets
 		volatile uint8_t tx_head=0;
 		volatile uint8_t tx_tail=0;
 	#endif // BASH_SERIAL
-	#ifdef SERIAL_DEBUG
-		volatile uint8_t tx_debug_buff[TXBUFFER_SIZE];
-		volatile uint8_t tx_debug_head=0;
-		volatile uint8_t tx_debug_tail=0;
-    #endif // SERIAL_DEBUG
 	uint8_t v_lipo1;
 	uint8_t v_lipo2;
 	uint8_t RX_RSSI;
@@ -215,8 +207,7 @@ void setup()
 {
 	// Setup diagnostic uart before anything else
 	#ifdef SERIAL_DEBUG
-		usart1_begin(115200,SERIAL_8N1);
-		tx_debug_resume();
+		Serial1.begin(115200,SERIAL_8N1);
 		debug("Multiprotocol version: %d.%d.%d.%d", VERSION_MAJOR, VERSION_MINOR, VERSION_REVISION, VERSION_PATCH_LEVEL);
 	#endif
 
@@ -701,18 +692,6 @@ inline void tx_resume()
 	#endif
 }
 
-#ifdef SERIAL_DEBUG
-	inline void tx_debug_resume()
-	{
-		USART1_BASE->CR1 |= USART_CR1_TXEIE;
-	}
-
-	inline void tx_debug_pause()
-	{
-		USART1_BASE->CR1 &= ~ USART_CR1_TXEIE;
-	}
-#endif // SERIAL_DEBUG
-
 #ifdef STM32_BOARD	
 void start_timer2()
 {	
diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino
index 9b8d9bf..04f0af0 100644
--- a/Multiprotocol/Telemetry.ino
+++ b/Multiprotocol/Telemetry.ino
@@ -732,19 +732,6 @@ void TelemetryUpdate()
 /**************************/
 /**************************/
 
-#ifdef SERIAL_DEBUG
-	void StatusSerial_write(uint8_t data)
-	{
-		uint8_t nextHead ;
-		nextHead = tx_debug_head + 1 ;
-		if ( nextHead >= TXBUFFER_SIZE )
-			nextHead = 0 ;
-		tx_debug_buff[nextHead]=data;
-		tx_debug_head = nextHead ;
-		tx_debug_resume();
-	}
-#endif // SERIAL_DEBUG
-
 #ifndef BASH_SERIAL
 	// Routines for normal serial output
 	void Serial_write(uint8_t data)
@@ -864,29 +851,6 @@ void TelemetryUpdate()
 		#endif		
 	}
 	#ifdef STM32_BOARD
-		#if defined(SERIAL_DEBUG)
-			void __irq_usart1()
-			{	// Transmit interrupt
-				if(USART1_BASE->SR & USART_SR_TXE)
-				{
-					if(tx_debug_head!=tx_debug_tail)
-					{
-						if(++tx_debug_tail>=TXBUFFER_SIZE)				//head
-							tx_debug_tail=0;
-						USART1_BASE->DR=tx_debug_buff[tx_debug_tail];	//clears TXE bit
-					}
-					if (tx_debug_tail == tx_debug_head)
-						tx_debug_pause(); // Check if all data is transmitted . if yes disable transmitter UDRE interrupt
-				}
-			}
-			void usart1_begin(uint32_t baud,uint32_t config )
-			{
-				usart_init(USART1);
-				usart_config_gpios_async(USART1,GPIOA,PIN_MAP[PA10].gpio_bit,GPIOA,PIN_MAP[PA9].gpio_bit,config);
-				usart_set_baud_rate(USART1, STM32_PCLK1, baud);
-				usart_enable(USART1);
-			}
-		#endif
 		void usart2_begin(uint32_t baud,uint32_t config )
 		{
 			usart_init(USART2);