diff --git a/communications/src/serial_console.c b/communications/src/serial_console.c
index 7842eb7d91bf8866bd014625a7a30f234339bbfd..64fa7f4ecc58828bdbcadd8d31eb4787ac52d085 100644
--- a/communications/src/serial_console.c
+++ b/communications/src/serial_console.c
@@ -3,21 +3,40 @@
 #include <stdio.h>
 
 /* private variables */
-volatile uint8_t serial_console_buffer[SERIAL_CONSOLE_MAX_BUFFER_LEN];
-volatile uint8_t serial_console_read_ptr;
-volatile uint8_t serial_console_write_ptr;
-volatile uint8_t serial_console_num_data;
+volatile uint8_t serial_console_rx_buffer[SERIAL_CONSOLE_MAX_BUFFER_LEN];
+volatile uint8_t serial_console_rx_read_ptr;
+volatile uint8_t serial_console_rx_write_ptr;
+volatile uint8_t serial_console_rx_num_data;
+volatile uint8_t serial_console_tx_buffer[SERIAL_CONSOLE_MAX_BUFFER_LEN];
+volatile uint8_t serial_console_tx_read_ptr;
+volatile uint8_t serial_console_tx_write_ptr;
+volatile uint8_t serial_console_tx_num_data;
+volatile uint8_t serial_console_sending;
 static FILE *device;
 
 /* interrupt handlers */
 SIGNAL(USART1_RX_vect)
 {
-  if(serial_console_num_data<SERIAL_CONSOLE_MAX_BUFFER_LEN)
+  cli();
+  serial_console_rx_buffer[serial_console_rx_write_ptr]=UDR1;
+  serial_console_rx_write_ptr=(serial_console_rx_write_ptr+1)%SERIAL_CONSOLE_MAX_BUFFER_LEN;
+  if(serial_console_rx_num_data<SERIAL_CONSOLE_MAX_BUFFER_LEN)
+    serial_console_rx_num_data++;
+  sei();
+}
+
+SIGNAL(USART1_TX_vect)
+{
+  cli();
+  if(serial_console_tx_num_data>0)
   {
-    serial_console_buffer[serial_console_write_ptr]=UDR1;
-    serial_console_write_ptr=(serial_console_write_ptr+1)%SERIAL_CONSOLE_MAX_BUFFER_LEN;
-    serial_console_num_data++;
+    UDR1=serial_console_tx_buffer[serial_console_tx_read_ptr];
+    serial_console_tx_read_ptr=(serial_console_tx_read_ptr+1)%SERIAL_CONSOLE_MAX_BUFFER_LEN;
+    serial_console_tx_num_data--;
   }
+  else
+    serial_console_sending=0x00;
+  sei();
 }
 
 /* private functions */
@@ -32,14 +51,25 @@ void serial_console_set_baudrate(uint32_t baudrate)
 
 int serial_console_putchar(char c,FILE *dev)
 {
-  if( c == '\n' )
+
+  cli();
+  if(c=='\n')
   {
-    while((UCSR1A&(1<<UDRE1))==0x00);
-    UDR1 = '\r';
+    serial_console_tx_buffer[serial_console_tx_write_ptr]='\r';
+    serial_console_tx_write_ptr=(serial_console_tx_write_ptr+1)%SERIAL_CONSOLE_MAX_BUFFER_LEN;
+    serial_console_tx_num_data++;
   }
-
-  while((UCSR1A&(1<<UDRE1))==0x00);
-  UDR1 = c;
+  serial_console_tx_buffer[serial_console_tx_write_ptr]=c;
+  serial_console_tx_write_ptr=(serial_console_tx_write_ptr+1)%SERIAL_CONSOLE_MAX_BUFFER_LEN;
+  serial_console_tx_num_data++;
+  if(serial_console_sending==0x00)
+  {
+    UDR1=serial_console_tx_buffer[serial_console_tx_read_ptr];
+    serial_console_tx_read_ptr=(serial_console_tx_read_ptr+1)%SERIAL_CONSOLE_MAX_BUFFER_LEN;
+    serial_console_tx_num_data--;
+    serial_console_sending=0x01;
+  }
+  sei();
 
   return 0;
 }
@@ -48,14 +78,22 @@ int serial_console_getchar(FILE *dev)
 {
   int8_t rx;
 
-  while(serial_console_num_data==0);
-  rx=serial_console_buffer[serial_console_read_ptr];
-  serial_console_read_ptr=(serial_console_read_ptr+1)%SERIAL_CONSOLE_MAX_BUFFER_LEN;
-  serial_console_num_data--;
-  if(rx=='\r')
-    rx='\n';
-
-  return rx;
+  cli();
+  if(serial_console_rx_num_data>0)
+  {
+    rx=serial_console_rx_buffer[serial_console_rx_read_ptr];
+    serial_console_rx_read_ptr=(serial_console_rx_read_ptr+1)%SERIAL_CONSOLE_MAX_BUFFER_LEN;
+    serial_console_rx_num_data--;
+    if(rx=='\r')
+      rx='\n';
+    sei();
+    return rx;
+  }
+  else
+  {
+    sei();
+    return -1;
+  }
 }
 
 /* pubic functions */
@@ -78,7 +116,7 @@ void serial_console_init(uint32_t baudrate)
   // bit4: enable rx
   // bit3: enable tx
   // bit2: set sendding size(0 = 8bit)
-  UCSR1B = 0b10011000;
+  UCSR1B = 0b11011000;
 
   // set UART register C
   // bit6: communication mode (1 = synchronize, 0 = asynchronize)
@@ -88,10 +126,13 @@ void serial_console_init(uint32_t baudrate)
   UCSR1C = 0b00000110;
 
   // initialize
-  UDR1 = 0xFF;
-  serial_console_read_ptr=0;
-  serial_console_write_ptr=0;
-  serial_console_num_data=0;
+  serial_console_rx_read_ptr=0;
+  serial_console_rx_write_ptr=0;
+  serial_console_rx_num_data=0;
+  serial_console_tx_read_ptr=0;
+  serial_console_tx_write_ptr=0;
+  serial_console_tx_num_data=0;
+  serial_console_sending=0x00;
 
   // set baudrate
   serial_console_set_baudrate(baudrate);