diff --git a/UAV-ControlSystem/inc/drivers/arduino_com.h b/UAV-ControlSystem/inc/drivers/arduino_com.h
index 64ce846..627692a 100644
--- a/UAV-ControlSystem/inc/drivers/arduino_com.h
+++ b/UAV-ControlSystem/inc/drivers/arduino_com.h
@@ -62,6 +62,13 @@ bool arduino_frame_available();
 ***********************************************************************/
 void arduino_read();
 
+
+/***********************************************************************
+* BRIEF: Update the output sensor values and sends them to the Arduino *
+* INFORMATION:             				                               *
+***********************************************************************/
+void arduino_send_sensor_values();
+
 #endif /* DRIVERS_ARDUINO_COM_H_ */
 
 
diff --git a/UAV-ControlSystem/src/drivers/arduino_com.c b/UAV-ControlSystem/src/drivers/arduino_com.c
index 594ff57..0068ea9 100644
--- a/UAV-ControlSystem/src/drivers/arduino_com.c
+++ b/UAV-ControlSystem/src/drivers/arduino_com.c
@@ -13,6 +13,40 @@
 
 #define COMPASS_PACKET_SIZE 8
 #define GPS_PACKET_SIZE 10
+#define ARDUINO_SENSOR_SIZE 6
+
+typedef struct arduino_sensor_t {
+	uint8_t 	ID     __attribute__((packed));
+	uint32_t 	value  __attribute__((packed));
+	uint8_t		crc    __attribute__((packed));
+} arduino_sensor_t;
+
+
+/* Some definitions of packets and sensor IDs */
+enum smartport_packets_e {
+	FSSP_START_STOP = 0x7E,	// Start/Stop bit sent from RX when polling
+	FSSP_DATA_FRAME = 0x10,	// Sensor replies with this as start byte
+
+	// ID of sensors. Must be something polled by FrSky RX
+	FSS_SENSOR_CURRENT     = 0xA1,
+	FSS_SENSOR_VOLTAGE     = 0x22,
+	FSS_SENSOR_BAROMETER   = 0x1B,
+	FSS_TUNE_PITCH         = 0x0D,
+	FSS_TUNE_ROLL          = 0x34,
+	FSS_SENSOR_6           = 0x67,
+};
+
+enum smartportID {
+	CURRENT_SENSOR_ID = 0,
+	VOLTAGE_SENSOR_ID,
+	BAROMETER_SENSOR_ID,
+	TUNE_PITCH_ID,
+	TUNE_ROLL_ID,
+	SENSOR_COUNT,
+};
+
+
+arduino_sensor_t sensors[SENSOR_COUNT];
 
 
 usart_dma_profile dmaHandler;
@@ -58,7 +92,22 @@ void arduinoCom_init(USART_TypeDef* usart_inst)
 {
 	/* initialize the USART with a dma buffer */
 	usart_init_dma(usart_inst, &dmaHandler, ARDUINO_BAUD, STOP_BITS_1, PARITY_NONE, ARDUINO_DMA_SIZE, 0);
-//	usart_transmit(&dmaHandler.usart_pro, "data", 4, 100000);
+
+	/*Initialize the sensors to be sent over smartport*/
+	sensors[CURRENT_SENSOR_ID].ID = FSS_SENSOR_CURRENT;
+	sensors[CURRENT_SENSOR_ID].value = 0;
+
+	sensors[VOLTAGE_SENSOR_ID].ID = FSS_SENSOR_VOLTAGE;
+	sensors[VOLTAGE_SENSOR_ID].value = 0;
+
+	sensors[BAROMETER_SENSOR_ID].ID = FSS_SENSOR_BAROMETER;
+	sensors[BAROMETER_SENSOR_ID].value = 0;
+
+	sensors[TUNE_PITCH_ID].ID = FSS_TUNE_PITCH;
+	sensors[TUNE_PITCH_ID].value = 0;
+
+	sensors[TUNE_ROLL_ID].ID = FSS_TUNE_ROLL;
+	sensors[TUNE_ROLL_ID].value = 0;
 }
 
 
@@ -198,3 +247,73 @@ void arduino_read()
 		}
 	}
 }
+
+uint8_t calculate_crc(uint8_t *data, uint8_t length)
+{
+	uint8_t crc = 0;
+	for(int i = 0; i < length; i++)
+		crc ^= data[i];
+
+	return crc;
+}
+
+/***********************************************************************
+* BRIEF: Update the output sensor values and calculate the crc         *
+* INFORMATION:             				                               *
+***********************************************************************/
+void update_sensor_values()
+{
+
+	/* TODO: Add the correct data to the value parameters here*/
+	sensors[CURRENT_SENSOR_ID].value += 1;
+	sensors[CURRENT_SENSOR_ID].crc = calculate_crc(&sensors[CURRENT_SENSOR_ID], ARDUINO_SENSOR_SIZE - 1);
+
+	sensors[VOLTAGE_SENSOR_ID].value += 2;
+	sensors[VOLTAGE_SENSOR_ID].crc = calculate_crc(&sensors[VOLTAGE_SENSOR_ID], ARDUINO_SENSOR_SIZE - 1);
+
+	sensors[BAROMETER_SENSOR_ID].value += 3;
+	sensors[BAROMETER_SENSOR_ID].crc = calculate_crc(&sensors[BAROMETER_SENSOR_ID], ARDUINO_SENSOR_SIZE - 1);
+
+	sensors[TUNE_PITCH_ID].value += 4;
+	sensors[TUNE_PITCH_ID].crc = calculate_crc(&sensors[TUNE_PITCH_ID], ARDUINO_SENSOR_SIZE - 1);
+
+	sensors[TUNE_ROLL_ID].value += 5;
+	sensors[TUNE_ROLL_ID].crc = calculate_crc(&sensors[TUNE_ROLL_ID], ARDUINO_SENSOR_SIZE - 1);
+}
+
+
+/***********************************************************************
+* BRIEF: Update the output sensor values and sends them to the Arduino *
+* INFORMATION:             				                               *
+***********************************************************************/
+void arduino_send_sensor_values()
+{
+	update_sensor_values();
+	for (int i = 0; i < SENSOR_COUNT; i++)
+	{
+		usart_transmit(&dmaHandler.usart_pro, (uint8_t *) &sensors[i], 6, 10000);
+	}
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c
index 18fbba9..fb73cea 100644
--- a/UAV-ControlSystem/src/main.c
+++ b/UAV-ControlSystem/src/main.c
@@ -65,8 +65,6 @@ void init_system()
 	sbus_init();
 
 
-
-
 #ifdef USE_LEDS
 	//Initialize the on board leds
 	ledReavoEnable();