diff --git a/UAV-ControlSystem/src/drivers/arduino_com.c b/UAV-ControlSystem/src/drivers/arduino_com.c
index d09c013..8e1394d 100644
--- a/UAV-ControlSystem/src/drivers/arduino_com.c
+++ b/UAV-ControlSystem/src/drivers/arduino_com.c
@@ -147,6 +147,7 @@ void arduino_parse_message(uint8_t data)
 				/* Clear necessary variables in order to fill the buffer with new ones */
 				message_it = 0;
 				find_header = true;
+				secondary_message_it = 0;
 				crc = 0;
 
 				memcpy(msg_header_and_size.dataPtr, arduino_arr, msg_header_and_size.size);
diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c
index b683c4c..7b5e199 100644
--- a/UAV-ControlSystem/src/main.c
+++ b/UAV-ControlSystem/src/main.c
@@ -56,16 +56,7 @@ void init_system()
 	cliInit(USART3);
 
 	//init sbus, using USART1
-	//sbus_init();
-
-	// TODO!! TEMP code
-//	arduinoCom_init(USART1);
-//	while (1) {
-//		if(arduino_frame_available())
-//			arduino_read();
-//		HAL_Delay(15);
-//	}
-
+	sbus_init();
 
 	//init motors to run with oneshot 125, small delay
 	HAL_Delay(1000);