diff --git a/Control/firmware/Bootloader/Bootloader/Bootloader.cproj b/Control/firmware/Bootloader/Bootloader/Bootloader.cproj
index 732540b..5c73799 100644
--- a/Control/firmware/Bootloader/Bootloader/Bootloader.cproj
+++ b/Control/firmware/Bootloader/Bootloader/Bootloader.cproj
@@ -30,15 +30,15 @@
     <EraseKey />
     <AsfFrameworkConfig>
       <framework-data>
-  <options />
-  <configurations />
-  <files />
-  <documentation help="" />
-  <offline-documentation help="" />
-  <dependencies>
-    <content-extension eid="atmel.asf" uuidref="Atmel.ASF" version="3.26.0" />
-  </dependencies>
-</framework-data>
+        <options />
+        <configurations />
+        <files />
+        <documentation help="" />
+        <offline-documentation help="" />
+        <dependencies>
+          <content-extension eid="atmel.asf" uuidref="Atmel.ASF" version="3.26.0" />
+        </dependencies>
+      </framework-data>
     </AsfFrameworkConfig>
     <avrtool>com.atmel.avrdbg.tool.atmelice</avrtool>
     <avrtoolserialnumber>J41800089261</avrtoolserialnumber>
@@ -69,7 +69,7 @@
   <avrgcc.compiler.symbols.DefSymbols>
     <ListValues>
       <Value>NDEBUG</Value>
-      <Value>ARDUINO_NANO=1</Value>
+      <Value>ARDUINO_NANO=0</Value>
     </ListValues>
   </avrgcc.compiler.symbols.DefSymbols>
   <avrgcc.compiler.directories.IncludePaths>
@@ -113,7 +113,7 @@
   <avrgcc.compiler.symbols.DefSymbols>
     <ListValues>
       <Value>DEBUG</Value>
-      <Value>ARDUINO_NANO=1</Value>
+      <Value>ARDUINO_NANO=0</Value>
     </ListValues>
   </avrgcc.compiler.symbols.DefSymbols>
   <avrgcc.compiler.directories.IncludePaths>
diff --git a/Control/firmware/uart_echo/adc.c b/Control/firmware/uart_echo/adc.c
index a610bfd..2989195 100644
--- a/Control/firmware/uart_echo/adc.c
+++ b/Control/firmware/uart_echo/adc.c
@@ -11,48 +11,58 @@
 #include "adc.h"
 #include "message.h"
 
-static volatile int16_t adc_value[16];
-static volatile size_t adc_ch = 0;
+static volatile uint8_t adc_ch = 0;
 static Fifo *g_pFifo = NULL;
+static Fifo g_conversionFifo;
+static int conversionBusy = 0;
 
 ISR(ADC_vect)
 {
+	conversionBusy = 0;
 	int16_t value_low = (int16_t)ADCL;
 	int16_t value_high = (int16_t)ADCH;
 	ADCSRA |= (1<<ADIF);
 	int16_t value = value_high << 8 | value_low;
-	adc_value[adc_ch] = value;
 
-	Msg_t msg = {AdcComplete, adc_getChannel()};
+	Msg_t msg = 
+	{
+		.code = AdcComplete,
+		.m.adc.ch = adc_ch,
+		.m.adc.data = value,
+	};
 	fifo_push(g_pFifo, &msg);
+	if (!fifo_isEmpty(&g_conversionFifo))
+	{
+		uint8_t ch;
+		fifo_pop(&g_conversionFifo, &ch);
+		adc_start_conversion(ch);
+	}
 }
 
 void adc_init(Fifo *pFifo)
 {
 	g_pFifo = pFifo;
+	fifo_init(&g_conversionFifo, 16, sizeof(uint8_t), "ConvFifo");
 	PRR &= ~(1 << PRADC);
 	ADCSRA |= (1 << ADEN) | 7;
 }
 
-void adc_start_conversion(size_t ch)
+void adc_start_conversion(uint8_t ch)
 {
 	ADMUX = (ADMUX & 0xF0) | (ch & 0x0F) | (1 << REFS0);
 	ADCSRA |= (1 << ADSC) | (1 << ADIE);
 	adc_ch = ch & 0x0F;
+	conversionBusy = 1;
 }
 
-int adc_is_fininshed()
+void adc_enqueue_conversion(uint8_t ch)
 {
-	return (int)(0 == (ADCSRA & (1 << ADSC)));
+	if (!conversionBusy)
+	{
+		adc_start_conversion(ch);
+	}
+	else
+	{
+		fifo_push(&g_conversionFifo, &ch);
+	}
 }
-
-size_t adc_getChannel()
-{
-	return adc_ch;
-}
-
-int16_t adc_getData()
-{
-	return adc_value[adc_ch];
-}
-
diff --git a/Control/firmware/uart_echo/adc.h b/Control/firmware/uart_echo/adc.h
index dc88dcc..7d32ed4 100644
--- a/Control/firmware/uart_echo/adc.h
+++ b/Control/firmware/uart_echo/adc.h
@@ -12,9 +12,7 @@
 #include "fifo.h"
 
 void adc_init(Fifo *pFifo);
-void adc_start_conversion(size_t ch);
-int adc_is_fininshed();
-size_t adc_getChannel();
-int16_t adc_getData();
+void adc_start_conversion(uint8_t ch);
+void adc_enqueue_conversion(uint8_t ch);
 
 #endif /* ADC_H_ */
\ No newline at end of file
diff --git a/Control/firmware/uart_echo/main.c b/Control/firmware/uart_echo/main.c
index ce6bd4d..e6ce520 100755
--- a/Control/firmware/uart_echo/main.c
+++ b/Control/firmware/uart_echo/main.c
@@ -32,8 +32,8 @@
 
 #define TIMER_SW_DELAY_MS(dly)	((dly*TIMER_IRQ_CLOCK)/1000UL)
 
-uint8_t ledState = 0;
-int16_t adc_last = 0;
+int16_t adc_poti = 0;
+int16_t adc_swin = 0;
 int sw_in_last = 0;
 size_t cmdStrSize = 0;
 Fifo messageFifo;
@@ -77,6 +77,7 @@ typedef enum _eState_t
 
 int main(void)
 {
+	int hb_state = 0;
 	cli();
     wdt_reset();
     wdt_disable();
@@ -124,8 +125,8 @@ int main(void)
 				{
 					// Process Uart message
 					cmdStr[cmdStrSize] = 0;
-					char c = msg.data;
-						
+					char c = (char)msg.m.uart.data;
+					
 					// Command finished with CR/LF
 					if (c == 0x0A || c == 0x0D)
 					{
@@ -178,57 +179,42 @@ int main(void)
 
 				case AdcComplete:
 				{
-					int16_t adc_curr = adc_getData();
-					if (adc_getChannel() == 6)
+					uint8_t adc_ch = msg.m.adc.ch;
+					int16_t adc_curr = (int16_t)msg.m.adc.data;
+					if (adc_ch == 0)
 					{
-						if (((adc_curr - adc_last) < -1) || ((adc_curr - adc_last) > 1))
+						if (((adc_curr - adc_poti) < -1) || ((adc_curr - adc_poti) > 1))
 						{
 							if (state == StateNormal)
 							{
 								setPower(adc_curr << 2);
 							}
+							adc_poti = adc_curr;
 						}
 					}
-					adc_last = adc_curr;
+					if (adc_ch == 2)
+					{
+						adc_swin = adc_curr;
+					}
 				}
 				break;
 
 				case Timer:
 				{
-					int timer_id = msg.data;
+					int timer_id = msg.m.timer.ch;
 					switch(timer_id)
 					{
 						case TIMER_GENERAL:
 						{
 							timer_start(timer_id, TIMER_SW_DELAY_MS(500));
-							switch(ledState)
-							{
-								case 0:
-								PORTB = (PORTB & 0xFC) | 0x00;
-								break;
-								case 1:
-								PORTB = (PORTB & 0xFC) | 0x01;
-								break;
-								case 2:
-								PORTB = (PORTB & 0xFC) | 0x03;
-								break;
-								case 3:
-								PORTB = (PORTB & 0xFC) | 0x02;
-								break;
-							}
-							ledState++;
-							if (ledState == 4)
-							{
-								ledState = 0;
-							}
+							hb_state = !hb_state;
+							portSet(Led_HB, hb_state);
 						}
 						break;
 						
 						case TIMER_TIMEOUT:
 						{
 							PRINT("Timeout!\n");
-							setPower(PowerLow);
-							setSwitch(SwitchOff);
 							state_next = StateError;
 						}
 						break;
@@ -236,10 +222,10 @@ int main(void)
 						case TIMER_ADC:
 						{
 							timer_start(timer_id, TIMER_SW_DELAY_MS(50));
-							int sw_in = portGet(PwrSwitch_in);
+							int sw_in = adc_swin > 512;
 							if (sw_in != sw_in_last)
 							{
-								PRINT ("PwrSwitch_in = %d\n", portGet(PwrSwitch_in));
+								PRINT ("PwrSwitch_in = %d\n", sw_in);
 								if (sw_in == 0)
 								{
 									if (state == StateError || state == StateRemote)
@@ -250,7 +236,8 @@ int main(void)
 								}
 							}
 							sw_in_last = sw_in;
-							adc_start_conversion(0x06);
+							adc_enqueue_conversion(0);
+							adc_enqueue_conversion(2);
 						}
 						break;
 					}
@@ -264,6 +251,16 @@ int main(void)
 		if (state != state_next)
 		{
 			PRINT("State change \"%s\" => \"%s\"\n", state_str[state], state_str[state_next]);
+			if (state_next == StateError)
+			{
+				setPower(PowerLow);
+				setSwitch(SwitchOff);
+				portSet(Led_ERR, 1);
+			}
+			if (state_next == StateNormal)
+			{
+				portSet(Led_ERR, 0);
+			}
 		}
 		state = state_next;
 
diff --git a/Control/firmware/uart_echo/message.h b/Control/firmware/uart_echo/message.h
index 5c300db..bc0206a 100644
--- a/Control/firmware/uart_echo/message.h
+++ b/Control/firmware/uart_echo/message.h
@@ -21,7 +21,22 @@ typedef enum _eMsgCode
 typedef struct _sMsg_t
 {
 	MsgCode code;
-	uint8_t data;		
+	union
+	{
+		struct
+		{
+			uint8_t data;
+		} uart;
+		struct
+		{
+			uint8_t ch;
+			uint16_t data;
+		} adc;
+		struct
+		{
+			uint8_t ch;
+		} timer;
+	} m;
 } Msg_t;
 
 #endif /* MESSAGE_H_ */
\ No newline at end of file
diff --git a/Control/firmware/uart_echo/port.c b/Control/firmware/uart_echo/port.c
index 31235c3..ab53daf 100644
--- a/Control/firmware/uart_echo/port.c
+++ b/Control/firmware/uart_echo/port.c
@@ -10,11 +10,10 @@
 
 void port_init()
 {
-	DDRB = 0x03;
-	DDRC = 0x01;
-	PORTB = 0x03;
-	PORTC = 0x01;
-	
+	DDRD = 0xFC;
+	DDRB = 0x06;
+	PORTD = 0xC0;
+	PORTB = 0x04;
 }
 
 typedef struct _sPort_t
@@ -26,11 +25,14 @@ typedef struct _sPort_t
 
 const Port_t portItems[NUM_ITEMS] = 
 {
-	{0x05, 0, 1},
-	{0x05, 1, 1},
 	{0x05, 2, 1},
-	{0x08, 0, 1},
-	{0x06, 1, 1},
+	{0x0B, 6, 1},
+	{0x0B, 7, 1},
+	{0x0B, 2, 1},
+	{0x0B, 3, 1},
+	{0x0B, 4, 1},
+	{0x0B, 5, 1},
+	{0x05, 1, 1},
 };
 
 void portSet(int portItem, int enable)
diff --git a/Control/firmware/uart_echo/port.h b/Control/firmware/uart_echo/port.h
index 2783a08..9a4e566 100644
--- a/Control/firmware/uart_echo/port.h
+++ b/Control/firmware/uart_echo/port.h
@@ -11,11 +11,14 @@
 
 enum PortItem
 {
-	Led0 = 0,
-	Led1 = 1,
-	Led2 = 2,
-	PwrSwitch_out = 3,
-	PwrSwitch_in = 4,
+	Led_HB = 0,
+	Led_MODE = 1,
+	Led_ERR = 2,
+	Led_0 = 3,
+	Led_1 = 4,
+	Led_2 = 5,
+	Led_3 = 6,
+	PwrSwitch_out = 7,
 	NUM_ITEMS
 };
 
diff --git a/Control/firmware/uart_echo/timer.c b/Control/firmware/uart_echo/timer.c
index 25f12f3..7969f60 100644
--- a/Control/firmware/uart_echo/timer.c
+++ b/Control/firmware/uart_echo/timer.c
@@ -28,7 +28,11 @@ ISR (TIMER1_OVF_vect)    // Timer1 ISR
 			}
 			else
 			{
-				Msg_t msg = {Timer, i};
+				Msg_t msg =
+				{
+					.code = Timer,
+					.m.timer.ch = i
+				};
 				if (fifo_push(g_pFifo, &msg))
 				{
 					user_timer[i].isRunning = 0;
diff --git a/Control/firmware/uart_echo/uart.c b/Control/firmware/uart_echo/uart.c
index 99467ff..b322e45 100644
--- a/Control/firmware/uart_echo/uart.c
+++ b/Control/firmware/uart_echo/uart.c
@@ -28,7 +28,11 @@ ISR(USART_RX_vect)
 	if (status & (1 << RXC0))
 	{
 		uint8_t rx_data = UDR0;
-		Msg_t msg = {Uart, rx_data};
+		Msg_t msg =
+		{
+			.code = Uart,
+			.m.uart.data = rx_data
+		};
 		fifo_push(g_pFifo, &msg);
 	}
 }
diff --git a/Control/firmware/uart_echo/uart_echo.cproj b/Control/firmware/uart_echo/uart_echo.cproj
index 7253569..b673962 100755
--- a/Control/firmware/uart_echo/uart_echo.cproj
+++ b/Control/firmware/uart_echo/uart_echo.cproj
@@ -95,7 +95,7 @@
   <avrgcc.compiler.symbols.DefSymbols>
     <ListValues>
       <Value>NDEBUG</Value>
-      <Value>ARDUINO_NANO=1</Value>
+      <Value>ARDUINO_NANO=0</Value>
     </ListValues>
   </avrgcc.compiler.symbols.DefSymbols>
   <avrgcc.compiler.directories.IncludePaths>
@@ -135,7 +135,7 @@
   <avrgcc.compiler.symbols.DefSymbols>
     <ListValues>
       <Value>DEBUG</Value>
-      <Value>ARDUINO_NANO=1</Value>
+      <Value>ARDUINO_NANO=0</Value>
     </ListValues>
   </avrgcc.compiler.symbols.DefSymbols>
   <avrgcc.compiler.directories.IncludePaths>