diff --git a/UAV-ControlSystem/inc/Flight/pid.h b/UAV-ControlSystem/inc/Flight/pid.h
index 39728c7..7213f7d 100644
--- a/UAV-ControlSystem/inc/Flight/pid.h
+++ b/UAV-ControlSystem/inc/Flight/pid.h
@@ -67,8 +67,7 @@ extern float accPitchFineTune;
 
 extern accel_t accelProfile;												/*Struct profile for input data from sensor*/
 
-/*Is set in motor mix and used in pidUAVcore and mix */
-bool motorLimitReached;
+
 
 /**************************************************************************
 * BRIEF: Initializes PID profiles			 					          *
diff --git a/UAV-ControlSystem/inc/config/eeprom.h b/UAV-ControlSystem/inc/config/eeprom.h
index 1774a35..7c1530c 100644
--- a/UAV-ControlSystem/inc/config/eeprom.h
+++ b/UAV-ControlSystem/inc/config/eeprom.h
@@ -208,6 +208,8 @@ typedef enum {
 typedef enum {
 	EEPROM_PID_GYRO,
 	EEPROM_PID_ACCELEROMETER,
+	EEPROM_PID_COMPASS,
+	EEPROM_PID_BAROMETER,
 
 	/* Counts the amount of settings in profile */
 	EEPROM_PROFILE_COUNT
diff --git a/UAV-ControlSystem/inc/drivers/accel_gyro.h b/UAV-ControlSystem/inc/drivers/accel_gyro.h
index 805964d..d1c6560 100644
--- a/UAV-ControlSystem/inc/drivers/accel_gyro.h
+++ b/UAV-ControlSystem/inc/drivers/accel_gyro.h
@@ -114,6 +114,7 @@ typedef struct accel_t {
 	int16_t accelXraw, accelYraw, accelZraw;  /* Raw accelerometer data */
 	int16_t offsetX, offsetY, offsetZ;        /* Accelerometer offset raw values */
 	uint16_t accel1G;                         /* Sensitivity factor */
+	float rollAngle, pitchAngle;
 } accel_t;
 
 /***********************************************************************
@@ -164,6 +165,8 @@ int mpu6000_read_fifo(gyro_t* gyro, int16_t* data_out);
  ***********************************************************************/
 bool mpu6000_who_am_i();
 
+void mpu6000_read_angle(accel_t* accel, gyro_t* gyro);
+
 #endif /* DRIVERS_ACCEL_GYRO_H_ */
 
 
diff --git a/UAV-ControlSystem/inc/drivers/arduino_com.h b/UAV-ControlSystem/inc/drivers/arduino_com.h
new file mode 100644
index 0000000..64ce846
--- /dev/null
+++ b/UAV-ControlSystem/inc/drivers/arduino_com.h
@@ -0,0 +1,78 @@
+/*
+ * arduino_com.h
+ *
+ *  Created on: 26 okt. 2016
+ *      Author: Philip
+ */
+
+#ifndef DRIVERS_ARDUINO_COM_H_
+#define DRIVERS_ARDUINO_COM_H_
+
+#include "drivers/usart.h"
+
+#define ARDUINO_BAUD 115200
+#define ARDUINO_DMA_SIZE 20
+
+
+/***********************************************************************
+* BRIEF: RX packet structure from arduino com                          *
+* INFORMATION: Contains the whole compass message 					   *
+***********************************************************************/
+typedef struct compass_data_t {
+	uint8_t header;
+	int16_t x;
+	int16_t y;
+	int16_t z;
+	uint8_t crc;
+} compass_data_t;
+
+/***********************************************************************
+* BRIEF: RX packet structure from arduino com                          *
+* INFORMATION: Contains the whole gps data message 					   *
+***********************************************************************/
+typedef struct gps_data_t {
+	uint8_t header;
+	float latitude;
+	float longitude;
+	uint8_t crc;
+} gps_data_t;
+
+/* An instance of the GPS data read from Arduino Com */
+gps_data_t gps_data;
+
+/* An instance of the compass data read from Arduino Com */
+compass_data_t compass_data;
+
+
+/***********************************************************************
+* BRIEF: Initializes the UART for Arduino com                          *
+* INFORMATION: A DMA Buffer starts storing the bytes received from RX  *
+***********************************************************************/
+void arduinoCom_init(USART_TypeDef* usart_inst);
+
+/***********************************************************************
+* BRIEF: Checks if new RX packet is available                          *
+* INFORMATION: Is called by the scheduler                              *
+***********************************************************************/
+bool arduino_frame_available();
+
+/***********************************************************************
+* BRIEF: Updates "gps_data" and "compass_data"         				   *
+* INFORMATION: Is called by the scheduler                              *
+***********************************************************************/
+void arduino_read();
+
+#endif /* DRIVERS_ARDUINO_COM_H_ */
+
+
+
+//----------------------- Example code for the parser --------------------------
+
+//arduinoCom_init(USART1);
+//while (1) {
+//	if(arduino_frame_available())
+//		arduino_read();
+//	HAL_Delay(15);
+//	float lng = gps_data.longitude;
+//	float lat = gps_data.latitude;
+//}
diff --git a/UAV-ControlSystem/inc/drivers/motormix.h b/UAV-ControlSystem/inc/drivers/motormix.h
index 85522b9..fc999e7 100644
--- a/UAV-ControlSystem/inc/drivers/motormix.h
+++ b/UAV-ControlSystem/inc/drivers/motormix.h
@@ -46,6 +46,9 @@ typedef struct  {
 /* Global mixerConfig to bee available to EEPROM */
 extern mixerConfig_s mixerConfig;
 
+/*Is set in motor mix and used in pidUAVcore and mix */
+extern bool motorLimitReached;
+
 /***********************************************************************
 * BRIEF: The motormixer                                                *
 * INFORMATION: Sums the output from all control loops and adapts the   *
diff --git a/UAV-ControlSystem/inc/utilities.h b/UAV-ControlSystem/inc/utilities.h
index 446ddea..1f0bba8 100644
--- a/UAV-ControlSystem/inc/utilities.h
+++ b/UAV-ControlSystem/inc/utilities.h
@@ -25,6 +25,8 @@
 
 #define maxStringSize_CLI  		100		//Max sting size used for the messages in the CLI
 
+#define ABS_FLOAT(x)	(((x) < 0)? -(x): (x))
+
 typedef char typeString[maxStringSize_CLI];
 typedef struct typeStringArr { char val[maxStringSize_CLI]; } typeStringArr;
 
@@ -81,6 +83,13 @@ uint32_t accumulate(uint32_t list[], int length);
 ***********************************************************************/
 void Error_Handler(void);
 
+/**************************************************************************
+* BRIEF: Constrain float values within a defined limit                    *
+* INFORMATION: Used in PID loop to limit values                           *
+**************************************************************************/
+float constrainf(float amt, int low, int high);
+
+
 uint8_t reverse(uint8_t byte);
 
 int16_t constrain(int16_t value, int16_t min, int16_t max);
diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c
index e538621..162ceed 100644
--- a/UAV-ControlSystem/src/Flight/pid.c
+++ b/UAV-ControlSystem/src/Flight/pid.c
@@ -1,6 +1,6 @@
 /**************************************************************************
 * NAME: pid.c      		                                                  *
-* AUTHOR: Johan Gärtner                                                   *
+* AUTHOR: Johan G�rtner                                                   *
 *
 * PURPOSE: This file contains pid functions		                          *
 * INFORMATION: pidUAV is the final pid controller which only takes        *
@@ -19,19 +19,20 @@
 #include <math.h>
 #include "drivers/failsafe_toggles.h"
 #include "drivers/motormix.h"
+#include "utilities.h"
 
 
 #define PTERM_SCALE 0.032029f										/*P-term used as a scale value to the PID controller*/
-#define ITERM_SCALE 0.0012f										/*I-term used as a scale value to the PID controller*/
+#define ITERM_SCALE 0.0012f											/*I-term used as a scale value to the PID controller*/
 #define DTERM_SCALE 0.000529f										/*D-term used as a scale value to the PID controller*/
 
 #define RADIO_RANGE 500												/*Radio range input*/
-#define BAROMETER_RANGE 2000										/*Determines the range of the maximum height (limits the rc input)*/
+#define BAROMETER_RANGE 3											/*Determines the range of the maximum height (limits the rc input)*/
 #define ACCELEROMETER_RANGE 60										/*Determines the range of the maximum angle  (limits the rc input) & (Accelerometer takes int to max 16 G)*/
 #define GYRO_RANGE 720												/*Determines the maximum rotational limit (limits the rc input)*/
 #define COMPASS_RANGE 180											/*Determines the maximum compass limit (limits the rc input)*/
 
-#define PID_MAX_I 256											/*Constrains ITerm*/
+#define PID_MAX_I 256												/*Constrains ITerm*/
 #define PID_MAX_D 512												/*Constrains DTerm*/
 
 /*Struct that belongs to a certain PID controller*/
@@ -59,43 +60,10 @@ gyro_t gyroProfile = {0};													/*Struct profile for input data from senso
 
 pt1Filter_t accelFilter[2] = {0};
 
+float throttleRate = 0;
 float accRollFineTune = 0;
 float accPitchFineTune = 0;
 
-/**************************************************************************
-* BRIEF: Calculates angle from accelerometer   		                      *
-* INFORMATION:                                                            *
-**************************************************************************/
-float calcAngle(const uint8_t axis, const float x_axis, const float y_axis, const float z_axis)
-{
-	float angle;
-
-	switch (axis)
-	{
-	case ROLL:
-
-		angle = atan2(x_axis, sqrt(y_axis*y_axis + z_axis*z_axis))*180/M_PI;
-		angle = -1*((angle > 0)? (z_axis < 0 )? 180 - angle: angle : (z_axis < 0 )? - 180 - angle: angle);
-
-		break;
-	case PITCH:
-
-		angle = atan2( y_axis, sqrt(z_axis*z_axis + x_axis*x_axis))*180/M_PI; 	/*down (the front down against ground) = pos angle*/
-		angle = (angle > 0)? ((z_axis < 0))? 180 - angle: angle : (z_axis < 0 )? - 180 - angle: angle;
-
-		break;
-	default:
-		angle = 0;
-		break;
-	}
-
-	return angle;
-}
-
-float calcGravity(accel_t profile ) //const float x_axis, const float y_axis, const float z_axis)
-{
-	return sqrt(profile.accelXconv*profile.accelXconv + profile.accelYconv*profile.accelYconv + profile.accelZconv*profile.accelZconv);
-}
 
 /**************************************************************************
 * BRIEF: Scales data from input range to output range                     *
@@ -104,38 +72,14 @@ float calcGravity(accel_t profile ) //const float x_axis, const float y_axis, co
 float convertData(int inputRange, int outputRange, int offset, float value)
 {
 	return ((float)outputRange/(float)inputRange)*(value-(float)offset);
-	//return 1.0;
 }
 
-/**************************************************************************
-* BRIEF: Constrain float values within a defined limit                    *
-* INFORMATION: Used in PID loop to limit values                           *
-**************************************************************************/
-float constrainf(float amt, int low, int high)
-{
-	if (amt < low)
-		return low;
-	else if (amt > high)
-		return high;
-	else
-		return amt;
-}
-
-
-float oldSensorValue[2] = {0};
-float oldSensorValueRoll[12] = {0};
-float oldSensorValuePitch[12] = {0};
-
-int i = 0;
-
 /**************************************************************************
 * BRIEF: Update current sensor values				                      *
 * INFORMATION:                       									  *
 **************************************************************************/
 void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
 {
-
-
 	switch (ID_profile)
 	{
 	case PID_ID_GYRO:
@@ -149,57 +93,27 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
 		break;
 	case PID_ID_ACCELEROMETER:
 
-		mpu6000_read_accel(&accelProfile);	/*Reads data from accelerometer*/
+		mpu6000_read_angle(&accelProfile,&gyroProfile);	/*Reads data from accelerometer*/
 
+		sensorValues[ROLL] = accelProfile.rollAngle + accRollFineTune;
+		sensorValues[PITCH] = accelProfile.pitchAngle + accPitchFineTune;
 
-		float alpha = 0.5;
-		/*May need Low pass filter since the accelerometer may drift*/
-
-		float X_roll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
-		float X_pitch = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
-
-		/*TODO add finetune for roll and pitch*/
-		X_roll += accRollFineTune;
-		X_pitch += accPitchFineTune;
-
-
-		oldSensorValueRoll[i] = X_roll;
-		oldSensorValuePitch[i] = X_pitch;
-
-		float RollValue = 0;
-		float PitchValue = 0;
-
-		for (int ii = 0; ii < 12; ii++)
-		{
-			RollValue = RollValue + oldSensorValueRoll[ii];
-			PitchValue = PitchValue + oldSensorValuePitch[ii];
-
-		}
-
-
-		i = (i < 11)? i + 1:0;
-
-		sensorValues[ROLL] = RollValue/12;
-		sensorValues[PITCH]  = PitchValue/12;
-
-		sensorValues[ROLL] = alpha*RollValue/12 + (1-alpha)*oldSensorValue[0];
-		sensorValues[PITCH] = alpha*PitchValue/12 + (1-alpha)*oldSensorValue[1];
-//
-		oldSensorValue[0] = sensorValues[ROLL];
-		oldSensorValue[1] = sensorValues[PITCH];
-
-//		sensorValues[ROLL] = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
-//		sensorValues[PITCH] = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
-
-		//float sensorRoll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
-		//sensorValues[ROLL] = pt1FilterApply4(&accelFilter[0], sensorRoll, 90, PidProfileBuff[ROLL].dT);
-
-		//float sensorPitch = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
-		//sensorValues[PITCH] = pt1FilterApply4(&accelFilter[1], sensorPitch, 90, PidProfileBuff[PITCH].dT);
+		/*Checks the biggest angle */
+		throttleRate = 2 - (ABS_FLOAT(sensorValues[ROLL]) > ABS_FLOAT(sensorValues[PITCH]) )? cos(sensorValues[ROLL]*M_PI/180) : cos(sensorValues[PITCH]*M_PI/180);
 
 		break;
 	case PID_ID_COMPASS:
+
+		sensorValues[ROLL] = 0;
+		sensorValues[PITCH] = 0;
+		sensorValues[YAW] = 0;
+
+
+		break;
 	case PID_ID_BAROMETER:
+
+
+		break;
 	default:
 
 		sensorValues[ROLL] = 0;
@@ -257,7 +171,9 @@ void getPointRate(float *desiredCommand, uint8_t ID_profile)
 		break;
 	case PID_ID_BAROMETER:
 
-		desiredCommand[THROTTLE] = convertData(RADIO_RANGE, BAROMETER_RANGE, 0, rc_input.Throttle);
+		//desiredCommand[THROTTLE] = convertData(RADIO_RANGE, BAROMETER_RANGE, 0, rc_input.Throttle*throttleRate);
+
+		desiredCommand[THROTTLE] = 1.9;
 
 		break;
 	default:
@@ -299,8 +215,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
 	float ITerm = pidProfileBuff->lastITerm[axis] + ITERM_SCALE * rateError * pidProfileBuff->dT * (float)pidProfile->I[axis];
 
 	// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
-	// I coefficient (I) moved before integration to make limiting independent from PID settings
-	ITerm = constrainf(ITerm, -PID_MAX_I, PID_MAX_I);
+	ITerm = constrainf(ITerm, -(int)PID_MAX_I, (int)PID_MAX_I);
 
 	// Anti windup protection
 	if (motorLimitReached)
@@ -309,7 +224,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
 	}
 	else
 	{
-		pidProfileBuff->ITermLimit[axis] = abs(ITerm);
+		pidProfileBuff->ITermLimit[axis] = ABS_FLOAT(ITerm);
 	}
 
 	pidProfileBuff->lastITerm[axis] = ITerm;
@@ -350,8 +265,9 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
 	{
 		ITerm = 0;
 		pidProfileBuff->lastITerm[axis] = 0;
+		pidProfileBuff->ITermLimit[axis] = 0;
 	}
-	pidProfile->PID_Out[axis] = constrainf(PTerm + ITerm + DTerm, -pidProfile->pid_out_limit, pidProfile->pid_out_limit);
+	pidProfile->PID_Out[axis] = constrainf(PTerm + ITerm + DTerm, -(int)pidProfile->pid_out_limit, (int)pidProfile->pid_out_limit);
 }
 
 /**************************************************************************
@@ -424,9 +340,9 @@ void pidRun(uint8_t ID)
 		break;
 	case PID_ID_BAROMETER:
 
-		if (!PidProfile[PID_ID_BAROMETER].pidEnabled)
+		if (!PidProfile[PID_ID_BAROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_barometer_id))
 		{
-			PidProfile[PID_ID_BAROMETER].PID_Out[0] = rc_input.Throttle;
+			PidProfile[PID_ID_BAROMETER].PID_Out[0] = rc_input.Throttle*throttleRate;
 		}
 		else
 		{
@@ -456,23 +372,23 @@ void pidUAVInitBuff(pidProfileBuff_t *pidProfile, uint8_t ID)
 	case PID_ID_GYRO:
 
 		PidProfileBuff[ID].DOF = 3;
-		PidProfileBuff[ID].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000; //ÄNDRA TILL SEKUNDER inte ms
+		PidProfileBuff[ID].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000000; //�NDRA TILL SEKUNDER inte ms
 
 		break;
 	case PID_ID_ACCELEROMETER:
 
 		PidProfileBuff[ID].DOF = 2;
-		PidProfileBuff[ID].dT = SystemTasks[TASK_ACCELEROMETER].desiredPeriod/1000;
+		PidProfileBuff[ID].dT = SystemTasks[TASK_ACCELEROMETER].desiredPeriod/1000000;
 
 		break;
 	case PID_ID_COMPASS:
 
-		PidProfileBuff[ID].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000;
+		PidProfileBuff[ID].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000000;
 
 		break;
 	case PID_ID_BAROMETER:
 
-		PidProfileBuff[ID].dT = SystemTasks[TASK_BARO].desiredPeriod/1000;
+		PidProfileBuff[ID].dT = SystemTasks[TASK_BARO].desiredPeriod/1000000;
 
 		break;
 	default:
@@ -598,7 +514,6 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
 void pidInit()
 {
 	mpu6000_init(&gyroProfile,&accelProfile); /*Init gyro and accelerometer*/
-	motorLimitReached = false;
 
 	pidUAVInitBuff(&PidProfileBuff[PID_ID_GYRO], PID_ID_GYRO);
 	pidUAVInitBuff(&PidProfileBuff[PID_ID_ACCELEROMETER], PID_ID_ACCELEROMETER);
@@ -614,8 +529,14 @@ void pidInit()
 void pidEproom(void)
 {
 
-	PidProfile[PID_ID_ACCELEROMETER].PIDweight[ROLL] = 2;
-	PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 2;
+	PidProfile[PID_ID_ACCELEROMETER].PIDweight[ROLL] = 4;
+	PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 4;
 	PidProfile[PID_ID_ACCELEROMETER].PIDweight[YAW] = 100;
 
+	PidProfileBuff[PID_ID_GYRO].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000; //�NDRA TILL SEKUNDER inte ms
+	PidProfileBuff[PID_ID_ACCELEROMETER].dT = SystemTasks[TASK_ACCELEROMETER].desiredPeriod/1000;
+	PidProfileBuff[PID_ID_COMPASS].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000;
+	PidProfileBuff[PID_ID_BAROMETER].dT = SystemTasks[TASK_BARO].desiredPeriod/1000;
+
+
 }
diff --git a/UAV-ControlSystem/src/config/cli.c b/UAV-ControlSystem/src/config/cli.c
index 1d7cb04..d77f8a9 100644
--- a/UAV-ControlSystem/src/config/cli.c
+++ b/UAV-ControlSystem/src/config/cli.c
@@ -31,7 +31,7 @@
 #define commandValueError		0xFFFFFFFFFFFFFFFF
 
 #define minSimilarCharacters	2			//the minimum amount of characters needed to to a search on a value
-#define maxSimilarSearchValues	15			//max amount of values that will be found when doing a search for similar strings based on the written chars
+#define maxSimilarSearchValues	18			//max amount of values that will be found when doing a search for similar strings based on the written chars
 #define CLI_baudRate			115200		//The baudrate used for the CLI usart
 #define msgArraySize 			3			//The number of words that a max command can contain (ex: set looptime 1000)
 
@@ -121,11 +121,11 @@ const typeString commandActionInformation_Strings[ACTION_COUNT] = {
 	"| reboot - Exit CLI and reboots the system.\n\r",
 	"| reset - Restore all the values to its default values.\n\r",
 	"| stats - Gives some current stats of the system and tasks.\n\r",
-	"| calibrate_motors - Calibrates all motors.",
-	"| calibrate_gyro - Calibrates the gyro.",
-	"| calibrate_accelerometer - Calibrates the accelerometer.",
-	"| calibrate_compass - Calibrates the compass.",
-	"| calibration_info_accelerometer - Provides info on the accelerometer calibration."
+	"| calibrate_motors - Calibrates all motors.\n\r",
+	"| calibrate_gyro - Calibrates the gyro.\n\r",
+	"| calibrate_accelerometer - Calibrates the accelerometer.\n\r",
+	"| calibrate_compass - Calibrates the compass.\n\r",
+	"| calibration_info_accelerometer - Provides info on the accelerometer calibration.\n\r"
 };
 
 /* String array containing all the signature examples for each action */
@@ -284,9 +284,22 @@ typedef enum
 	COMMAND_ID_PID_ACCEL_YAW_P_LIMIT,
 	COMMAND_ID_PID_ACCEL_OUT_LIMIT,
 
+	//Barometer
+	COMMAND_ID_PID_BARO_P_HEIGHT,
+
+	COMMAND_ID_PID_BARO_I_HEIGHT,
+
+	COMMAND_ID_PID_BARO_D_HEIGHT,
+
+	COMMAND_ID_PID_BARO_DTERM_LPF,
+	COMMAND_ID_PID_BARO_PTERM_YAW_LPF,
+	COMMAND_ID_PID_BARO_YAW_P_LIMIT,
+	COMMAND_ID_PID_BARO_OUT_LIMIT,
+
 	/* Enable the different pid loops */
 	COMMAND_ID_PID_GYRO_ISENABLED,
 	COMMAND_ID_PID_ACCEL_ISENABLED,
+	COMMAND_ID_PID_BARO_ISENABLED,
 
 	/* Counter for the amount of commands */
 	COMMAND_ID_COUNT,
@@ -659,6 +672,41 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = {
 				"pid_accel_out_limit", 	COMMAND_ID_PID_ACCEL_OUT_LIMIT, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 20, VAL_UINT_16, .valueRange = {0, 65000}
 		},
 
+		//BARO P
+		[COMMAND_ID_PID_BARO_P_HEIGHT] =
+		{
+				"pid_baro_p_height", 	COMMAND_ID_PID_BARO_P_HEIGHT, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 5, VAL_UINT_8, .valueRange = {0, 255}
+		},
+
+		//BARO I
+		[COMMAND_ID_PID_ACCEL_I_PITCH] =
+		{
+				"pid_baro_i_height", 	COMMAND_ID_PID_BARO_I_HEIGHT, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 8, VAL_UINT_8, .valueRange = {0, 255}
+		},
+		//BARO D
+		[COMMAND_ID_PID_ACCEL_D_PITCH] =
+		{
+				"pid_baro_d_height", 	COMMAND_ID_PID_ACCEL_D_PITCH, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 11, VAL_UINT_8, .valueRange = {0, 255}
+		},
+
+		//BARO Filters and limits
+		[COMMAND_ID_PID_BARO_DTERM_LPF] =
+		{
+				"pid_baro_dterm_lpf", 	COMMAND_ID_PID_BARO_DTERM_LPF, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 14, VAL_UINT_16, .valueRange = {0, 65000}
+		},
+		[COMMAND_ID_PID_BARO_PTERM_YAW_LPF] =
+		{
+				"pid_baro_pterm_yaw_lpf", 	COMMAND_ID_PID_BARO_PTERM_YAW_LPF, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 16, VAL_UINT_16, .valueRange = {0, 65000}
+		},
+		[COMMAND_ID_PID_BARO_YAW_P_LIMIT] =
+		{
+				"pid_baro_yaw_p_limit", 	COMMAND_ID_PID_BARO_YAW_P_LIMIT, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 18, VAL_UINT_16, .valueRange = {0, 65000}
+		},
+		[COMMAND_ID_PID_BARO_OUT_LIMIT] =
+		{
+				"pid_baro_out_limit", 	COMMAND_ID_PID_BARO_OUT_LIMIT, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 20, VAL_UINT_16, .valueRange = {0, 65000}
+		},
+
 		/* Enable pid loops */
 		[COMMAND_ID_PID_GYRO_ISENABLED] =
 		{
@@ -668,6 +716,10 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = {
 		{
 				"pid_accel_isenabled", 	COMMAND_ID_PID_ACCEL_ISENABLED, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_BOOL, .valueRange = {0, 1}
 		},
+		[COMMAND_ID_PID_BARO_ISENABLED] =
+		{
+				"pid_baro_isenabled", 	COMMAND_ID_PID_BARO_ISENABLED, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_BOOL, .valueRange = {0, 1}
+		},
 
 
 };
diff --git a/UAV-ControlSystem/src/config/eeprom.c b/UAV-ControlSystem/src/config/eeprom.c
index 7852cc1..d727865 100644
--- a/UAV-ControlSystem/src/config/eeprom.c
+++ b/UAV-ControlSystem/src/config/eeprom.c
@@ -291,6 +291,16 @@ EEPROM_DATA_t eeprom_profile_Arr[EEPROM_PROFILE_COUNT] = {
 		.size = sizeof(pidProfile_t),
 		.dataPtr = &(PidProfile[PID_ID_ACCELEROMETER]),
 	},
+	[EEPROM_PID_COMPASS] =
+	{
+		.size = sizeof(pidProfile_t),
+		.dataPtr = &(PidProfile[PID_ID_COMPASS]),
+	},
+	[EEPROM_PID_BAROMETER] =
+	{
+		.size = sizeof(pidProfile_t),
+		.dataPtr = &(PidProfile[PID_ID_BAROMETER]),
+	},
 };
 
 /* Data pointers and sizes for footer content */
diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c
index 759dcc5..471f835 100644
--- a/UAV-ControlSystem/src/drivers/accel_gyro.c
+++ b/UAV-ControlSystem/src/drivers/accel_gyro.c
@@ -7,6 +7,9 @@
 
 #include <drivers/accel_gyro.h>
 #include "drivers/spi.h"
+#include "utilities.h"
+#include "math.h"
+#include "drivers/system_clock.h"
 
 spi_profile mpu6000_spi_profile;
 uint8_t num_failed_receive = 0;
@@ -243,6 +246,8 @@ bool mpu6000_init(gyro_t* gyro, accel_t* accel)
 
 	HAL_Delay(60);
 
+	accel->pitchAngle = 0;
+	accel->rollAngle = 0;
 
 	return true;
 }
@@ -514,3 +519,79 @@ bool mpu6000_who_am_i()
 
 	return false;
 }
+
+
+
+
+
+/* Set the Gyro Weight for Gyro/Acc complementary filter
+   Increasing this value would reduce and delay Acc influence on the output of the filter*/
+#define GYRO_ACC_DIV_FACTOR (2^8) //  that means a CMP_FACTOR of 1024 (2^10)
+
+#define GetMagnitude(x)		(x*x)
+#define Low_Magnitude		(GetMagnitude(0.85))
+#define High_Magnitude		(GetMagnitude(1.15))
+
+/* Returns the angle. If magnitude of acc is out of range we calculate on integrated gyro data */
+void mpu6000_read_angle(accel_t* accel, gyro_t* gyro)
+{
+	static uint32_t last_micros = 0;			// Static stores micros measured from last iteration
+	uint32_t current_micros = clock_get_us();
+	uint32_t delta_t = current_micros - last_micros;
+	last_micros = current_micros;
+
+	float deltaGyroAngleFloat[3] = {0};
+	static float lpf_Acc[3] = {0};
+	static float smooth[3] = {0};
+	float sign[3] = {0};
+	float magnitude = 0;
+
+	/* We read the accelerometer to get fresh data */
+	mpu6000_read_accel(accel);
+	float accelConv[3] = {accel->accelXconv, accel->accelYconv, accel->accelZconv};
+
+	/* Filter part, go thorugh each axis */
+	for (int i = 0; i < 3; i ++)
+	{
+		//Calculate a new smooth value based on a factor of the LPF value
+		smooth[i] = lpf_Acc[i] / 16;
+
+		//Save the sign(+/-) of the value
+		sign[i] = (accelConv[i]< 0) ? -1 : 1;
+
+		//Calculate the new LPF value based on the raw sensor data - the smoothing value
+		lpf_Acc[i] += sign[i]*sqrtf(ABS_FLOAT(accelConv[i])) - smooth[i];
+
+		//recalculate the accelerometer data based on the smooth value, since we had to take the square root off the original value we square it to get in the original size
+		accelConv[i] = smooth[i] * smooth[i] * sign[i];
+
+		//calculate the magnitude of the gravitation for all axis
+		magnitude += ABS_FLOAT(accelConv[i]) * ABS_FLOAT(accelConv[i]);
+
+	}
+
+	//Calculate the approximate angle increase based on the gyros probable movement since the last invoke
+	deltaGyroAngleFloat[0] = (delta_t * (float)gyro->gyroX / 1000000.0);
+	deltaGyroAngleFloat[1] = (delta_t * (float)gyro->gyroY / 1000000.0) ;
+	deltaGyroAngleFloat[2] = (delta_t * (float)gyro->gyroZ / 1000000.0);
+
+	//First integrate the gyro and add that to the angle calculation
+	accel->rollAngle 	+= deltaGyroAngleFloat[1];
+	accel->pitchAngle 	+= deltaGyroAngleFloat[0];
+
+	//If the g forces of the accelerometer is within the given magnitude we will also add accelerometer data to the calculation
+	if (Low_Magnitude < magnitude && magnitude < High_Magnitude)
+	{
+		//Calculate the pure angle given by the accelerometer data
+		float a_RollAngle 	 = -atan2(accelConv[0], sqrt(accelConv[1]*accelConv[1] + accelConv[2]*accelConv[2]))*180/M_PI;
+		float a_PitchAngle	 = atan2( accelConv[1], sqrt(accelConv[2]*accelConv[2] + accelConv[0]*accelConv[0]))*180/M_PI;
+
+		//Check how much the accelerometer angle differs from the current calculated ange with the gyro. Add calculated factor to the real angle value
+		accel->rollAngle 	 += (a_RollAngle - accel->rollAngle) / GYRO_ACC_DIV_FACTOR;
+		accel->pitchAngle	 += (a_PitchAngle - accel->pitchAngle) / GYRO_ACC_DIV_FACTOR;
+	}
+
+	//Always constran the angular values within the possible ranges 90 to -90
+	accel->rollAngle = constrainf(accel->rollAngle, -90,90);
+	accel->pitchAngle = constrainf(accel->pitchAngle, -90,90);
+}
diff --git a/UAV-ControlSystem/src/drivers/arduino_com.c b/UAV-ControlSystem/src/drivers/arduino_com.c
new file mode 100644
index 0000000..594ff57
--- /dev/null
+++ b/UAV-ControlSystem/src/drivers/arduino_com.c
@@ -0,0 +1,200 @@
+/*
+ * arduino_com.c
+ *
+ *  Created on: 26 okt. 2016
+ *      Author: Philip
+ */
+
+#include "drivers/arduino_com.h"
+
+#include "utilities.h"
+#include "string.h"
+#include "stm32f4xx_revo.h"
+
+#define COMPASS_PACKET_SIZE 8
+#define GPS_PACKET_SIZE 10
+
+
+usart_dma_profile dmaHandler;
+dma_usart_return raw_dma_data_t;
+
+// enumeration to hold the id:s of the different packages
+enum packet_ids {
+	COMPASS_PACKET_ID 	= 0xA1,
+	GPS_PACKET_ID 		= 0xB1,
+};
+
+// Structure used to hold the data for "data_arr"
+typedef struct arduino_data_t {
+	uint8_t size;	//Size of the data
+	void * dataPtr;	//pointer to the data
+} arduino_data_t ;
+
+// An enumeration of the array positions of the data in the "data_arr"
+enum arduino_data_e {
+	COMPASS_DATA_ID,
+	GPS_DATA_ID,
+	ARDUINO_DATA_COUNT,
+};
+
+// An array to hold the pointers to the different data structures that are used in the rest of the system;
+arduino_data_t data_arr[ARDUINO_DATA_COUNT] = {
+		[COMPASS_DATA_ID] = {
+			.size = COMPASS_PACKET_SIZE,
+			.dataPtr = &compass_data,
+		},
+		[GPS_DATA_ID] = {
+			.size = GPS_PACKET_SIZE,
+			.dataPtr = &gps_data,
+		},
+};
+
+
+/***********************************************************************
+* BRIEF: Initializes the UART for Arduino com                          *
+* INFORMATION: A DMA Buffer starts storing the bytes received from RX  *
+***********************************************************************/
+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);
+}
+
+
+/***********************************************************************
+* BRIEF: Checks if new RX packet is available                          *
+* INFORMATION: Is called by the scheduler                              *
+***********************************************************************/
+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;
+}
+
+/***********************************************************************
+* BRIEF: Private function that looks for the correct data struct from  *
+* 		 the header data                                               *
+* INFORMATION:                                                         *
+***********************************************************************/
+arduino_data_t find_packet_from_header(uint8_t header)
+{
+	arduino_data_t arduino_data = {
+			.dataPtr = NULL,
+			.size = 0,
+	};
+
+	/* Check what header it is and return the correct datapointer if correct header*/
+	switch (header)
+	{
+		case COMPASS_PACKET_ID:
+			arduino_data = data_arr[COMPASS_DATA_ID];
+			break;
+		case GPS_PACKET_ID:
+			arduino_data = data_arr[GPS_DATA_ID];
+			break;
+		default:
+			break;
+	}
+	return arduino_data;
+}
+
+
+/***********************************************************************
+* BRIEF: A function that parses the message byte per byte and then
+* 		 if it was wrong it will do recursive call and parse the rest
+* 		 of the message
+* INFORMATION:                                                         *
+***********************************************************************/
+void arduino_parse_message(uint8_t data)
+{
+	static uint8_t arduino_arr[ARDUINO_DMA_SIZE];
+	static bool find_header = true;
+	static uint8_t message_it = 0;
+	static uint8_t secondary_message_it = 0;
+	static arduino_data_t msg_header_and_size = { .size = 0, .dataPtr = NULL };
+	static uint8_t crc = 0;
+
+	if(find_header)
+	{
+		msg_header_and_size = find_packet_from_header(data);
+
+		if(msg_header_and_size.size != 0)
+		{
+			find_header = false;
+			arduino_arr[(message_it)] = data;
+			message_it++;
+			crc ^= data;
+		}
+	}
+	else
+	{
+		/* If we find any new possible header then we should be able to return to that point in time */
+		if (((arduino_data_t)find_packet_from_header(data)).size != 0  && secondary_message_it == 0)
+		{
+			secondary_message_it = message_it;	//save the value of the position in The buffer array, not the dma array index
+		}
+
+		/* Reading the message except the crc byte */
+		if ((message_it) < (msg_header_and_size.size - 1))
+		{
+			arduino_arr[(message_it)] = data;
+			crc ^= data;
+			message_it++;
+		}
+		else if ((message_it) == (msg_header_and_size.size - 1))
+		{
+			/* put the crc code into the data buffer as well */
+			arduino_arr[(message_it)] = data;
+
+			/* TODO: Replace with check for CRC */
+			if (crc == 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);
+			}
+			else
+			{
+				int size = msg_header_and_size.size;
+				int new_iter = secondary_message_it;
+
+				crc = 0;
+				find_header = true;
+				message_it = 0;
+				secondary_message_it = 0;
+
+				if(new_iter > 0)
+				{
+					for(int i = new_iter; i < size; i++)
+					{
+						arduino_parse_message(arduino_arr[i]);
+					}
+				}
+			}
+		}
+	}
+}
+
+/***********************************************************************
+* BRIEF: Updates "gps_data" and "compass_data"         				   *
+* INFORMATION: Is called by the scheduler                              *
+***********************************************************************/
+void arduino_read()
+{
+	//If the DMA has come to a new buffer
+	if (raw_dma_data_t.new_data)
+	{
+		// parse the entire message to the gps_data and compass_data
+		for (int i = 0; i < ARDUINO_DMA_SIZE; i++)
+		{
+			arduino_parse_message(raw_dma_data_t.buff[i]);
+		}
+	}
+}
diff --git a/UAV-ControlSystem/src/drivers/failsafe_toggles.c b/UAV-ControlSystem/src/drivers/failsafe_toggles.c
index 58b00ce..2821950 100644
--- a/UAV-ControlSystem/src/drivers/failsafe_toggles.c
+++ b/UAV-ControlSystem/src/drivers/failsafe_toggles.c
@@ -28,7 +28,7 @@ boolFlags_t systemFlags = {{0}};
 /* Array of flag configurations. These are values that can be set by RC, should be read from eeprom. */
 flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT] = {
 		[FLAG_CONFIGURATION_ARM] = {
-			.minRange = 1500,
+			.minRange = 1800,
 			.maxRange = 2100,
 			.channelNumber = 8,
 			.flagId = systemFlags_armed_id,
diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c
index 1261837..0def06d 100644
--- a/UAV-ControlSystem/src/drivers/motormix.c
+++ b/UAV-ControlSystem/src/drivers/motormix.c
@@ -42,6 +42,9 @@
 /* An array containing the calculated motor outputs */
 uint16_t motor_output[MOTOR_COUNT];
 
+/* Bool to see if motors are maxed out. Stops windup in PID implementation */
+bool motorLimitReached = false;
+
 /* Default values for the mixerConfig */
 // TODO: Implement in EEPROM
 mixerConfig_s mixerConfig = {
@@ -102,8 +105,6 @@ void mix()
 	int16_t RPY_Mix_Max = 0;			// Maximum desired command for any motor
 	int16_t throttle = PidProfile[PID_ID_BAROMETER].PID_Out[THROTTLE];	 // Import throttle value from remote
 
-	// Might be used for some debug if necessary
-	//static bool motorLimitReached;
 
 	/* Mixer Full Scale enabled */
 	if (flags_IsSet_ID(systemFlags_mixerfullscale_id))
diff --git a/UAV-ControlSystem/src/drivers/motors.c b/UAV-ControlSystem/src/drivers/motors.c
index 53bb569..1a44d5c 100644
--- a/UAV-ControlSystem/src/drivers/motors.c
+++ b/UAV-ControlSystem/src/drivers/motors.c
@@ -14,6 +14,7 @@
 #include "drivers/motors.h"
 #include "drivers/failsafe_toggles.h"
 #include "config/eeprom.h"
+#include "drivers/motormix.h"
 
 const int MotorPWMPeriode = 2000; //Micro seconds
 const int MotorPWMInitPulse = 1000;
@@ -162,7 +163,11 @@ void pwmEnableMotor(uint8_t motor, motorOutput motorOutput)
 **************************************************************************/
 void pwmEnableAllMotors(motorOutput motorOutput)
 {
-	 for (uint8_t i = 1; i < 11; i++ ) pwmEnableMotor(i, motorOutput);
+	 for (uint8_t i = 1; i < 11; i++ )
+	 {
+		 pwmEnableMotor(i, motorOutput);
+		 pwmAdjustSpeedOfMotor(i,mixerConfig.minCommand);
+	 }
 }
 
 /**************************************************************************
diff --git a/UAV-ControlSystem/src/drivers/sbus.c b/UAV-ControlSystem/src/drivers/sbus.c
index f61c0b0..ccd9474 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;
@@ -122,16 +121,12 @@ void sbus_read()
 	// If continue only if we get new data from DMA
 	if (raw_dma_data_t.new_data)
 	{
-
-
-
 		for (int i = 0; i < USART1_SBUS_DMA_SIZE; i++)
 		{
 			uint8_t msg = raw_dma_data_t.buff[i];
 			// Look for the beginning of a sbus frame
 			if ( message_it == 0 ) //&& stop_bit_read)
 			{
-				//message_it = (raw_dma_data_t.buff[i] == ((uint8_t)SBUS_HEADER)) ? 1 : 0;
 				if (msg == ((uint8_t)SBUS_HEADER))
 				{
 					sbus_arr[(message_it)] = msg;
@@ -142,12 +137,7 @@ void sbus_read()
 				{
 					message_it = 0;
 				}
-
-//				sbus_arr_iterator = 0;
-//				stop_bit_read = false;
 			}
-			// Look for the end of sbus frame
-			//else if(raw_dma_data_t.buff[i] == (uint8_t)SBUS_FOOTER)
 			else
 			{
 				if (msg == (uint8_t)SBUS_HEADER && new_header == false)
@@ -169,10 +159,6 @@ void sbus_read()
 					{
 						message_it = 0;
 						missedMsg--;
-					//stop_bit_read = true;
-					// If the expected byte is stop byte, then we overwrite to the return value.
-					//if (sbus_arr_iterator == SBUS_FRAME_SIZE - 1)
-					//{
 						sbusChannelData = *(sbusFrame_s*)sbus_arr;
 
 						// Linear fitting
@@ -229,17 +215,10 @@ void sbus_read()
 								message_it_secondary_head = innerCount;
 							}
 						}
-
 					}
 				}
 			}
-
-//			// Copy next byte into the sbus_arr
-//			if (sbus_arr_iterator < SBUS_FRAME_SIZE)
-//				sbus_arr[sbus_arr_iterator] = raw_dma_data_t.buff[i];
-//			sbus_arr_iterator++;
 		}
-
 	}
 }
 
diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c
index 4441435..cb10d8f 100644
--- a/UAV-ControlSystem/src/main.c
+++ b/UAV-ControlSystem/src/main.c
@@ -28,8 +28,7 @@
 #include "drivers/motormix.h"
 #include "drivers/motors.h"
 #include "Flight/pid.h"
-#include "drivers/barometer.h"
-
+#include "drivers/barometer.h"#include "drivers/arduino_com.h"
 
 /**************************************************************************
 * BRIEF: Should contain all the initializations of the system, needs to
@@ -124,6 +123,7 @@ int main(void)
 	//Initialize the scheduler, add all the tasks that should run to the ready queue of the scheduler
 	initScheduler();
 
+
 	while (1)
 	{
 		//Run the scheduler, responsible for distributing all the work of the running system
diff --git a/UAV-ControlSystem/src/utilities.c b/UAV-ControlSystem/src/utilities.c
index 7c2192c..4e824d0 100644
--- a/UAV-ControlSystem/src/utilities.c
+++ b/UAV-ControlSystem/src/utilities.c
@@ -228,3 +228,17 @@ int16_t constrain(int16_t value, int16_t min, int16_t max)
 	else
 		return value;
 }
+
+/**************************************************************************
+* BRIEF: Constrain float values within a defined limit                    *
+* INFORMATION: Used in PID loop to limit values                           *
+**************************************************************************/
+float constrainf(float amt, int low, int high)
+{
+	if (amt < (float)low)
+		return (float)low;
+	else if (amt > (float)high)
+		return (float)high;
+	else
+		return amt;
+}