diff --git a/UAV-ControlSystem/inc/utilities.h b/UAV-ControlSystem/inc/utilities.h
index 446ddea..b8fa784 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,8 @@ uint32_t accumulate(uint32_t list[], int length);
 ***********************************************************************/
 void Error_Handler(void);
 
+
+
 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 87370fd..c16c8ec 100644
--- a/UAV-ControlSystem/src/Flight/pid.c
+++ b/UAV-ControlSystem/src/Flight/pid.c
@@ -19,6 +19,7 @@
 #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*/
@@ -317,19 +318,19 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
 	ITerm = constrainf(ITerm, -(int)PID_MAX_I, (int)PID_MAX_I);
 
 	// Anti windup protection
-//	if (motorLimitReached)
-//	{
-//		ITerm = constrainf(ITerm, -pidProfileBuff->ITermLimit[axis], pidProfileBuff->ITermLimit[axis]);
-//	}
-//	else
-//	{
-//		pidProfileBuff->ITermLimit[axis] = abs(ITerm);
-//	}
+	if (motorLimitReached)
+	{
+		ITerm = constrainf(ITerm, -pidProfileBuff->ITermLimit[axis], pidProfileBuff->ITermLimit[axis]);
+	}
+	else
+	{
+		pidProfileBuff->ITermLimit[axis] = ABS_FLOAT(ITerm);
+	}
 
-		if (motorLimitReached)
-		{
-			ITerm = pidProfileBuff->lastITerm[axis];
-		}
+//		if (motorLimitReached)
+//		{
+//			ITerm = pidProfileBuff->lastITerm[axis];
+//		}
 
 
 	pidProfileBuff->lastITerm[axis] = ITerm;
@@ -645,6 +646,4 @@ void pidEproom(void)
 
 	PidProfile[PID_ID_GYRO].I[YAW] = 40;
 
-
-
 }
diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c
index 2c9dfaf..bdefb67 100644
--- a/UAV-ControlSystem/src/main.c
+++ b/UAV-ControlSystem/src/main.c
@@ -124,6 +124,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