diff --git a/UAV-ControlSystem/.custom.cfg b/UAV-ControlSystem/.custom.cfg
new file mode 100644
index 0000000..c77723e
--- /dev/null
+++ b/UAV-ControlSystem/.custom.cfg
@@ -0,0 +1,13 @@
+# This is an .custom board with a single STM32F411RETx chip.
+# Generated by System Workbench for STM32
+
+source [find interface/stlink-v2-1.cfg]
+
+set WORKAREASIZE 0x20000
+transport select "hla_swd"
+
+
+source [find target/stm32f4x_stlink.cfg]
+
+# use hardware reset, connect under reset
+reset_config srst_only srst_nogate
diff --git a/UAV-ControlSystem/inc/drivers/arduino_com.h b/UAV-ControlSystem/inc/drivers/arduino_com.h
index 627692a..e9d739c 100644
--- a/UAV-ControlSystem/inc/drivers/arduino_com.h
+++ b/UAV-ControlSystem/inc/drivers/arduino_com.h
@@ -19,11 +19,11 @@
 * 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;
+	uint8_t header  __attribute__((packed));
+	int16_t x		__attribute__((packed));
+	int16_t y		__attribute__((packed));
+	int16_t z		__attribute__((packed));
+	uint8_t crc		__attribute__((packed));
 } compass_data_t;
 
 /***********************************************************************
@@ -31,10 +31,10 @@ typedef struct compass_data_t {
 * INFORMATION: Contains the whole gps data message 					   *
 ***********************************************************************/
 typedef struct gps_data_t {
-	uint8_t header;
-	float latitude;
-	float longitude;
-	uint8_t crc;
+	uint8_t header	__attribute__((packed));
+	float latitude;	__attribute__((packed))
+	float longitude	__attribute__((packed));
+	uint8_t crc		__attribute__((packed));
 } gps_data_t;
 
 /* An instance of the GPS data read from Arduino Com */
diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c
index 9585fb2..d7e44ea 100644
--- a/UAV-ControlSystem/src/Flight/pid.c
+++ b/UAV-ControlSystem/src/Flight/pid.c
@@ -133,7 +133,7 @@ float convertData(int inputRange, int outputRange, int offset, float 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)
+float constrainfu(float amt, int low, int high)
 {
 	if (amt < (float)low)
 		return (float)low;
@@ -181,14 +181,6 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
 		/*Checks the biggest angle */
 		throttleRate = (ABS_FLOAT(sensorValues[ROLL]) > ABS_FLOAT(sensorValues[PITCH]) )? 2 - cos(sensorValues[ROLL]*M_PI/180) : 2 - 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:
 
@@ -208,59 +200,62 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
 		break;
 	case PID_ID_COMPASS:
 	{
-		float xMagnetFiltered = 0;
-		float yMagnetFiltered = 0;
-		float zMagnetFiltered = 0;
-		xMagnetFiltered = MagnetFilteredOld[0] + alphaMagnet * (compass_data.x - MagnetFilteredOld[0]);
-		yMagnetFiltered = MagnetFilteredOld[1] + alphaMagnet * (compass_data.y - MagnetFilteredOld[1]);
-		zMagnetFiltered = MagnetFilteredOld[2] + alphaMagnet * (compass_data.z - MagnetFilteredOld[2]);
-
-		MagnetFilteredOld[0] = xMagnetFiltered;
-		MagnetFilteredOld[1] = yMagnetFiltered;
-		MagnetFilteredOld[2] = zMagnetFiltered;
-
-
-		//this part is required to normalize the magnetic vector
-		if (xMagnetFiltered>MagnetMax[0]) { MagnetMax[0] = xMagnetFiltered; }
-		if (yMagnetFiltered>MagnetMax[1]) { MagnetMax[1] = yMagnetFiltered; }
-		if (zMagnetFiltered>MagnetMax[2]) { MagnetMax[2] = zMagnetFiltered; }
-
-		if (xMagnetFiltered<MagnetMin[0]) { MagnetMin[0] = xMagnetFiltered; }
-		if (yMagnetFiltered<MagnetMin[1]) { MagnetMin[1] = yMagnetFiltered; }
-		if (zMagnetFiltered<MagnetMin[2]) { MagnetMin[2] = zMagnetFiltered; }
-
-		float norm;
-
-		MagnetMap[0] = (float)(map(xMagnetFiltered, MagnetMin[0], MagnetMax[0], -10000, 10000)) / 10000.0;
-		MagnetMap[1] = (float)(map(yMagnetFiltered, MagnetMin[1], MagnetMax[1], -10000, 10000)) / 10000.0;
-		MagnetMap[2] = (float)(map(zMagnetFiltered, MagnetMin[2], MagnetMax[2], -10000, 10000)) / 10000.0;
-
-		//normalize the magnetic vector
-		norm = sqrt(sq(MagnetMap[0]) + sq(MagnetMap[1]) + sq(MagnetMap[2]));
-		MagnetMap[0] /= norm;
-		MagnetMap[1] /= norm;
-		MagnetMap[2] /= norm;
-
-//		compare Applications of Magnetic Sensors for Low Cost Compass Systems by Michael J. Caruso
-//		for the compensated Yaw equations...
-//		http://www.ssec.honeywell.com/magnetic/datasheets/lowcost.pdf
-		Yaw = atan2(-(MagnetMap[1] * cos(oldSensorValue[0]) + MagnetMap[2] * sin(oldSensorValue[0])), MagnetMap[0] * cos(oldSensorValue[1]) + MagnetMap[1] * sin(oldSensorValue[1]) * sin(oldSensorValue[0]) + MagnetMap[2] * sin(oldSensorValue[1]) * cos(oldSensorValue[0]));
-		YawU = atan2(-MagnetMap[1], MagnetMap[0]);
-
-//		float XH;
-//		XH = compass_data.y * cos(oldSensorValue[1]) +
-//			 compass_data.x * sin(oldSensorValue[1]) * sin(oldSensorValue[0]) +
-//			 compass_data.z * sin(oldSensorValue[1]) * cos(oldSensorValue[0]);
-//		float YH;
-//		YH = compass_data.x * cos(oldSensorValue[0]) +
-//			 compass_data.z * sin(oldSensorValue[0]);
+		readAcc();
+//		float xMagnetFiltered = 0;
+//		float yMagnetFiltered = 0;
+//		float zMagnetFiltered = 0;
+//		xMagnetFiltered = MagnetFilteredOld[0] + alphaMagnet * (compass_data.x - MagnetFilteredOld[0]);
+//		yMagnetFiltered = MagnetFilteredOld[1] + alphaMagnet * (compass_data.y - MagnetFilteredOld[1]);
+//		zMagnetFiltered = MagnetFilteredOld[2] + alphaMagnet * (compass_data.z - MagnetFilteredOld[2]);
 //
-//		Yaw = atan2(-YH, XH);
-//		YawU = atan2(compass_data.y, compass_data.x);
+//		MagnetFilteredOld[0] = xMagnetFiltered;
+//		MagnetFilteredOld[1] = yMagnetFiltered;
+//		MagnetFilteredOld[2] = zMagnetFiltered;
+//
+//
+//		//this part is required to normalize the magnetic vector
+//		if (xMagnetFiltered>MagnetMax[0]) { MagnetMax[0] = xMagnetFiltered; }
+//		if (yMagnetFiltered>MagnetMax[1]) { MagnetMax[1] = yMagnetFiltered; }
+//		if (zMagnetFiltered>MagnetMax[2]) { MagnetMax[2] = zMagnetFiltered; }
+//
+//		if (xMagnetFiltered<MagnetMin[0]) { MagnetMin[0] = xMagnetFiltered; }
+//		if (yMagnetFiltered<MagnetMin[1]) { MagnetMin[1] = yMagnetFiltered; }
+//		if (zMagnetFiltered<MagnetMin[2]) { MagnetMin[2] = zMagnetFiltered; }
+//
+//		float norm;
+//
+//		MagnetMap[0] = (float)(map(xMagnetFiltered, MagnetMin[0], MagnetMax[0], -10000, 10000)) / 10000.0;
+//		MagnetMap[1] = (float)(map(yMagnetFiltered, MagnetMin[1], MagnetMax[1], -10000, 10000)) / 10000.0;
+//		MagnetMap[2] = (float)(map(zMagnetFiltered, MagnetMin[2], MagnetMax[2], -10000, 10000)) / 10000.0;
+//
+//		//normalize the magnetic vector
+//		norm = sqrt(sq(MagnetMap[0]) + sq(MagnetMap[1]) + sq(MagnetMap[2]));
+//		MagnetMap[0] /= norm;
+//		MagnetMap[1] /= norm;
+//		MagnetMap[2] /= norm;
+//
+////		compare Applications of Magnetic Sensors for Low Cost Compass Systems by Michael J. Caruso
+////		for the compensated Yaw equations...
+////		http://www.ssec.honeywell.com/magnetic/datasheets/lowcost.pdf
+//		Yaw = atan2(-(MagnetMap[1] * cos(accelProfile.rollAngle) +
+//					  MagnetMap[2] * sin(accelProfile.rollAngle)),
+//					  MagnetMap[0] * cos(accelProfile.pitchAngle) +
+//					  MagnetMap[1] * sin(accelProfile.pitchAngle) * sin(accelProfile.rollAngle) +
+//					  MagnetMap[2] * sin(accelProfile.rollAngle) * cos(accelProfile.pitchAngle));
+//		YawU = atan2(MagnetMap[1], MagnetMap[0]);
 
+		float XH;
+		XH = (compass_data.y * cos(accelProfile.pitchAngle * (3.141592 / 180))) +
+			 (compass_data.x * sin(accelProfile.pitchAngle * (3.141592 / 180)) * sin(accelProfile.rollAngle * (3.141592 / 180))) +
+			 (compass_data.z * cos(accelProfile.pitchAngle * (3.141592 / 180)) * sin(accelProfile.rollAngle * (3.141592 / 180)));
+		float YH;
+		YH = (compass_data.x * cos(accelProfile.rollAngle * (3.141592 / 180))) +
+			 (compass_data.z * sin(accelProfile.rollAngle * (3.141592 / 180)));
+
+		Yaw = atan2f(-YH, XH);
+		YawU = atan2f(-compass_data.x, compass_data.y);
 	}
 	break;
-	case PID_ID_BAROMETER:
 	default:
 		current_micros = clock_get_us();
 		current_micros = current_micros/1000000;
@@ -360,7 +355,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
 		/*Limits the PTerm of the Yaw axis */
 		if (pidProfile->yaw_p_limit)
 		{
-			PTerm = constrainf(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
+			PTerm = constrainfu(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
 		}
 	}
 
@@ -368,12 +363,12 @@ 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.
-	ITerm = constrainf(ITerm, -(int)PID_MAX_I, (int)PID_MAX_I);
+	ITerm = constrainfu(ITerm, -(int)PID_MAX_I, (int)PID_MAX_I);
 
 	// Anti windup protection
 	if (motorLimitReached)
 	{
-		ITerm = constrainf(ITerm, -pidProfileBuff->ITermLimit[axis], pidProfileBuff->ITermLimit[axis]);
+		ITerm = constrainfu(ITerm, -pidProfileBuff->ITermLimit[axis], pidProfileBuff->ITermLimit[axis]);
 	}
 	else
 	{
@@ -409,7 +404,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
 		}
 
 		DTerm = DTERM_SCALE * delta * (float)pidProfile->D[axis] * (float)pidProfile->PIDweight[axis] / 100.0;
-		DTerm = constrainf(DTerm, -PID_MAX_D, PID_MAX_D);
+		DTerm = constrainfu(DTerm, -PID_MAX_D, PID_MAX_D);
 	}
 
 
@@ -420,7 +415,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
 		pidProfileBuff->lastITerm[axis] = 0;
 		pidProfileBuff->ITermLimit[axis] = 0;
 	}
-	pidProfile->PID_Out[axis] = constrainf(PTerm + ITerm + DTerm, -(int)pidProfile->pid_out_limit, (int)pidProfile->pid_out_limit);
+	pidProfile->PID_Out[axis] = constrainfu(PTerm + ITerm + DTerm, -(int)pidProfile->pid_out_limit, (int)pidProfile->pid_out_limit);
 }
 
 /**************************************************************************
@@ -522,11 +517,11 @@ void pidRun(uint8_t ID)
 		else
 		{
 			pidUAV(&PidProfile[PID_ID_BAROMETER], &PidProfileBuff[PID_ID_BAROMETER]);
-			PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainf(PidProfile[PID_ID_BAROMETER].PID_Out[0], -20, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit);
+			PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainfu(PidProfile[PID_ID_BAROMETER].PID_Out[0], -20, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit);
 
 
-			//PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainf(PidProfile[PID_ID_BAROMETER].PID_Out[0], -15, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit);
-			//PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainf(PidProfile[PID_ID_BAROMETER].PID_Out[0], -(int)PidProfile[PID_ID_BAROMETER].pid_out_limit, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit);
+			//PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainfu(PidProfile[PID_ID_BAROMETER].PID_Out[0], -15, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit);
+			//PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainfu(PidProfile[PID_ID_BAROMETER].PID_Out[0], -(int)PidProfile[PID_ID_BAROMETER].pid_out_limit, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit);
 		}
 
 		break;