diff --git a/Control/firmware/uart_echo/main.c b/Control/firmware/uart_echo/main.c
index 1592a27..37dbcbf 100755
--- a/Control/firmware/uart_echo/main.c
+++ b/Control/firmware/uart_echo/main.c
@@ -17,13 +17,9 @@
 #else
 #define F_CPU			(18432000UL)	// MHz
 #endif
-#define USART_BAUDRATE	9600UL
+#define USART_BAUDRATE	38400UL
 #define BAUD_PRESCALE	(((F_CPU / (USART_BAUDRATE * 16UL))) - 1)
 #define TIMER_RELOAD	(0xFFFF-(F_CPU/1024/4-1))
-static uint8_t rx_data;
-static uint8_t count = 0;
-volatile uint8_t msgCode = 0;
-static uint8_t msgData = 0;
 static uint8_t ledState = 0;
 static int16_t adc_value[16];
 static size_t adc_ch = 0;
@@ -120,6 +116,7 @@ int Fifo_isEmpty(Fifo *pObj)
 	return pObj->fill == 0;
 }
 
+void adc_start_conversion(int ch);
 ISR(ADC_vect)
 {
 	int16_t value_low = (int16_t)ADCL;
@@ -144,19 +141,17 @@ ISR(ADC_vect)
 
 ISR(USART_RX_vect)
 {
-	rx_data = UDR0;
-	
-	msgCode = NOP;
+	uint8_t rx_data = UDR0;
 	if (rx_data == ' ')
 	{
-		msgCode = Error;
+		Msg_t msg = {Error, rx_data};
+		Fifo_push(&messageFifo, &msg);
 	} 
 	else if (rx_data >= '0' &&  rx_data <= '9')
 	{
 		Msg_t msg = {Command, rx_data};
 		Fifo_push(&messageFifo, &msg);
 	}
-	count++;
 }
 
 ISR (TIMER1_OVF_vect)    // Timer1 ISR