diff --git a/UAV-ControlSystem/inc/drivers/arduino_com.h b/UAV-ControlSystem/inc/drivers/arduino_com.h
new file mode 100644
index 0000000..4c1d0fc
--- /dev/null
+++ b/UAV-ControlSystem/inc/drivers/arduino_com.h
@@ -0,0 +1,16 @@
+/*
+ * arduino_com.h
+ *
+ *  Created on: 26 okt. 2016
+ *      Author: Philip
+ */
+
+#ifndef DRIVERS_ARDUINO_COM_H_
+#define DRIVERS_ARDUINO_COM_H_
+
+#define ARDUINO_BAUD 115200
+#define ARDUINO_DMA_SIZE 100
+
+void arduinoCom_init(USART_TypeDef usart);
+
+#endif /* DRIVERS_ARDUINO_COM_H_ */
diff --git a/UAV-ControlSystem/src/drivers/arduino_com.c b/UAV-ControlSystem/src/drivers/arduino_com.c
new file mode 100644
index 0000000..6d4df13
--- /dev/null
+++ b/UAV-ControlSystem/src/drivers/arduino_com.c
@@ -0,0 +1,172 @@
+/*
+ * arduino_com.c
+ *
+ *  Created on: 26 okt. 2016
+ *      Author: Philip
+ */
+
+#include "drivers/arduino_com.h"
+#include "drivers/usart.h"
+#include "utilities.h"
+#include "string.h"
+#include "stm32f4xx_revo.h"
+
+usart_dma_profile dmaHandler;
+dma_usart_return raw_dma_data_t;
+
+enum packet_ids {
+	COMPASS_PACKET_ID 	= 0xA1,
+	GPS_PACKET_ID 		= 0xB1,
+};
+
+typedef struct compass_data_t {
+	const uint8_t header = COMPASS_PACKET_ID;
+	int16_t x;
+	int16_t y;
+	int16_t z;
+	uint8_t crc;
+} compass_data_t;
+
+compass_data_t compass_data = {0};
+
+typedef struct gps_data_t {
+	const uint8_t header = GPS_PACKET_ID;
+	float latitude;
+	float longitude;
+	uint8_t crc;
+} gps_data_t;
+
+gps_data_t gps_data = {0};
+
+typedef struct arduino_data_t {
+	uint8_t size;	//Size of the data
+	void * dataPtr;	//pointer to the data
+} arduino_data_t ;
+
+enum arduino_data_e {
+	COMPASS_DATA_ID,
+	GPS_DATA_ID,
+	ARDUINO_DATA_COUNT,
+};
+
+arduino_data_t data_arr[ARDUINO_DATA_COUNT] = {
+		[COMPASS_DATA_ID] = {
+			.size = sizeof(compass_data),
+			.dataPtr = &compass_data,
+		},
+		[GPS_DATA_ID] = {
+			.size = sizeof(gps_data),
+			.dataPtr = &gps_data,
+		},
+};
+
+
+void arduinoCom_init(USART_TypeDef usart)
+{
+	usart_init_dma(usart, &dmaHandler, ARDUINO_BAUD, STOP_BITS_2, PARITY_EVEN, ARDUINO_DMA_SIZE, 0);
+}
+
+
+bool arduino_frame_available()
+{
+	/* We read data from DMA */
+	raw_dma_data_t = usart_get_dma_buffer(&dmaHandler);
+
+	return raw_dma_data_t.new_data;
+}
+
+void arduino_read()
+{
+	static uint8_t arduino_arr[ARDUINO_DMA_SIZE];
+	static uint8_t message_it = 0;
+	static uint32_t missedMsg = 0;
+	static uint8_t message_it_secondary_head = 0;
+	static bool new_header = false;
+	static uint8_t current_packet_size = 0;
+
+	if (raw_dma_data_t.new_data)
+	{
+
+		for (int i = 0; i < ARDUINO_DMA_SIZE; i++)
+			{
+				uint8_t msg = raw_dma_data_t.buff[i];
+				// Look for the beginning of a frame
+				if ( message_it == 0 )
+				{
+					switch (msg)
+					{
+					case COMPASS_PACKET_ID:
+						current_packet_size = data_arr[COMPASS_DATA_ID].size;
+						arduino_arr[(message_it)] = msg;
+						message_it++;
+						new_header = false;
+						break;
+					case GPS_PACKET_ID:
+						current_packet_size = data_arr[GPS_DATA_ID].size;
+						arduino_arr[(message_it)] = msg;
+						message_it++;
+						new_header = false;
+						break;
+					default:
+						message_it = 0;
+
+					}
+
+				}
+				// Look for the end of sbus frame
+				else
+				{
+					if (msg == (uint8_t)SBUS_HEADER && new_header == false)
+					{
+						new_header = true;
+						message_it_secondary_head = message_it;	//save the value of the position in The buffer array, not the dma array index
+					}
+
+					if ((message_it) < SBUS_FRAME_SIZE)
+					{
+						sbus_arr[(message_it)] = msg;
+						message_it++;
+					}
+
+					if ((message_it) == SBUS_FRAME_SIZE)
+					{
+						missedMsg++;
+						if (msg == (uint8_t)SBUS_FOOTER)
+						{
+							message_it = 0;
+							missedMsg--;
+							sbusChannelData = *(sbusFrame_s*)sbus_arr;
+						}
+					}
+					else
+					{
+						int temp_secondaryHeader = message_it_secondary_head;
+						message_it = message_it - temp_secondaryHeader;	//update the counter to the empty part of the updated array
+						new_header = false;		//set new header to false, this is true if there is another header within the buffer
+
+						//Move all the remaning messages in the buffer to the start of the buffer
+						for (int i = temp_secondaryHeader; i < SBUS_FRAME_SIZE; i++)
+						{
+							int innerCount = i-temp_secondaryHeader;
+							sbus_arr[innerCount] = sbus_arr[i];
+
+							//check if we find another possible header inside the rest of the buffer and save that
+							if (sbus_arr[innerCount] == (uint8_t)SBUS_HEADER && innerCount > 0 && new_header == false )
+							{
+								new_header = true;
+								message_it_secondary_head = innerCount;
+							}
+						}
+
+					}
+				}
+
+			}
+
+
+	}
+
+
+
+
+}
diff --git a/UAV-ControlSystem/src/drivers/sbus.c b/UAV-ControlSystem/src/drivers/sbus.c
index f61c0b0..c58955d 100644
--- a/UAV-ControlSystem/src/drivers/sbus.c
+++ b/UAV-ControlSystem/src/drivers/sbus.c
@@ -28,7 +28,6 @@
 
 /* This instance is read by the whole system and should contain actual RX data */
 sbusFrame_s sbusChannelData = {0};
-dma_usart_return raw_dma_data_t;
 dma_usart_return raw_dma_data_t = {0};
 rc_input_t rc_input = {0};
 float rc_rate = 1.0;