diff --git a/Control/firmware/uart_echo/adc.c b/Control/firmware/uart_echo/adc.c
index d051fbe..a610bfd 100644
--- a/Control/firmware/uart_echo/adc.c
+++ b/Control/firmware/uart_echo/adc.c
@@ -11,8 +11,8 @@
 #include "adc.h"
 #include "message.h"
 
-static int16_t adc_value[16];
-static size_t adc_ch = 0;
+static volatile int16_t adc_value[16];
+static volatile size_t adc_ch = 0;
 static Fifo *g_pFifo = NULL;
 
 ISR(ADC_vect)
diff --git a/Control/firmware/uart_echo/fifo.c b/Control/firmware/uart_echo/fifo.c
index 58f183a..389389d 100644
--- a/Control/firmware/uart_echo/fifo.c
+++ b/Control/firmware/uart_echo/fifo.c
@@ -5,6 +5,7 @@
  *  Author: jens
  */ 
 
+#include <stdint.h>
 #include "fifo.h"
 
 void fifo_init(Fifo *pObj, size_t capacity, size_t itemSize, const char *pName)
@@ -40,7 +41,10 @@ int fifo_push(Fifo *pObj, void *pItem)
 	{
 		return 0;
 	}
-	memcpy(pObj->ppData[pObj->wi], pItem, pObj->itemSize);
+	for (size_t i=0; i < pObj->itemSize; i++)
+	{
+		((uint8_t*)pObj->ppData[pObj->wi])[i] = ((uint8_t*)pItem)[i];
+	}
 	pObj->wi++;
 	pObj->fill++;
 	if (pObj->wi == pObj->capacity)
@@ -56,7 +60,10 @@ int fifo_pop(Fifo *pObj, void *pItem)
 	{
 		return 0;
 	}
-	memcpy(pItem, pObj->ppData[pObj->ri], pObj->itemSize);
+	for (size_t i=0; i < pObj->itemSize; i++)
+	{
+		((uint8_t*)pItem)[i] = ((uint8_t*)pObj->ppData[pObj->ri])[i];
+	}
 	pObj->ri++;
 	pObj->fill--;
 	if (pObj->ri == pObj->capacity)
diff --git a/Control/firmware/uart_echo/fifo.h b/Control/firmware/uart_echo/fifo.h
index eff8f77..664c287 100644
--- a/Control/firmware/uart_echo/fifo.h
+++ b/Control/firmware/uart_echo/fifo.h
@@ -10,16 +10,15 @@
 #define FIFO_H_
 
 #include <stdlib.h>
-#include <string.h>
 
 typedef struct _sFifo
 {
 	void **ppData;
 	size_t capacity;
 	size_t itemSize;
-	size_t fill;
-	size_t wi;
-	size_t ri;
+	size_t volatile fill;
+	size_t volatile wi;
+	size_t volatile ri;
 	const char *pName;
 } Fifo;
 
diff --git a/Control/firmware/uart_echo/main.c b/Control/firmware/uart_echo/main.c
index 2460a19..999a3ed 100755
--- a/Control/firmware/uart_echo/main.c
+++ b/Control/firmware/uart_echo/main.c
@@ -66,7 +66,7 @@ typedef enum _ePower
 
 void setPower(uint16_t power)
 {
-	printf("Set Power to %d\n", power);
+	printf("Set Power to %u\n", power);
 	uint8_t dac_cmd[] = {0x00, 0x00};					// 6.2 Write Volatile Memory (C2:C0 = ‘010’)
 	size_t res = MCP47x6_write_volatile_dac(power, dac_cmd, sizeof(dac_cmd));
 	i2c_send(0x60, I2C_WRITE, dac_cmd, res);
@@ -133,22 +133,23 @@ int main(void)
 				{
 					// Process Uart message
 					cmdStr[cmdStrSize] = 0;
-					if (cmdStrSize < sizeof(cmdStr))
-					{
-						char c = msg.data;
+					char c = msg.data;
 						
-						if (state != StateNormal && state != StateRemote)
+					// Command finished with CR/LF
+					if (c == 0x0A || c == 0x0D)
+					{
+						PRINT_PROMPT;
+						if (cmdStrSize < 2)
 						{
 							break;
 						}
 
-						// Command finished with CR/LF
-						if (c == 0x0A || c == 0x0D)
+						if (state == StateNormal || state == StateRemote)
 						{
 							// Power Control
 							if (toupper(cmdStr[0]) == 'P')
 							{
-								uint16_t arg = atol(cmdStr+1);
+								uint16_t arg = (uint16_t)strtol(cmdStr+1, (char **)NULL, 10);
 								setPower(arg);
 								timer_start(TIMER_TIMEOUT, TIMER_SW_DELAY_MS(5000));
 								state_next = StateRemote;
@@ -157,24 +158,21 @@ int main(void)
 							// Switch Control
 							if (toupper(cmdStr[0]) == 'S')
 							{
-								uint16_t arg = atol(cmdStr+1);
+								uint16_t arg = (uint16_t)strtol(cmdStr+1, (char **)NULL, 10);
 								setSwitch(arg);
 								timer_start(TIMER_TIMEOUT, TIMER_SW_DELAY_MS(5000));
 								state_next = StateRemote;
 							}
-							cmdStrSize = 0;
-							PRINT_PROMPT;
-						}
-						else
-						{
-							cmdStr[cmdStrSize++] = c;
-							printf("%c", c);
 						}
+						cmdStrSize = 0;
 					}
 					else
 					{
-						printf("Invalid command : %s\n", cmdStr);
-						cmdStrSize = 0;
+						if (cmdStrSize < (sizeof(cmdStr)-1))
+						{
+							printf("%c", c);
+							cmdStr[cmdStrSize++] = c;
+						}
 					}
 				}
 				break;
diff --git a/Control/firmware/uart_echo/uart.c b/Control/firmware/uart_echo/uart.c
index 70d133b..cb738a1 100644
--- a/Control/firmware/uart_echo/uart.c
+++ b/Control/firmware/uart_echo/uart.c
@@ -38,10 +38,12 @@ void uart_init(Fifo *pFifo, uint16_t BAUD_PRESCALE)
 
 void uart_putc(char c)
 {
+	cli();
 	while((UCSR0A & (1<<UDRE0)) == 0)
 	{
 	}
 	UDR0 = c;
+	sei();
 }
 
 int uart_putchar(char c, FILE *stream)