diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c
index c595d16..e538621 100644
--- a/UAV-ControlSystem/src/Flight/pid.c
+++ b/UAV-ControlSystem/src/Flight/pid.c
@@ -18,18 +18,20 @@
 #include "scheduler/scheduler.h"
 #include <math.h>
 #include "drivers/failsafe_toggles.h"
+#include "drivers/motormix.h"
+
 
 #define PTERM_SCALE 0.032029f										/*P-term used as a scale value to the PID controller*/
-#define ITERM_SCALE 0.244381f										/*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 ACCELEROMETER_RANGE 35										/*Determines the range of the maximum angle  (limits the rc input) & (Accelerometer takes int to max 16 G)*/
+#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*/
@@ -67,7 +69,6 @@ float accPitchFineTune = 0;
 float calcAngle(const uint8_t axis, const float x_axis, const float y_axis, const float z_axis)
 {
 	float angle;
-	float angle_offset = (z_axis < 0 )? 90: 0;
 
 	switch (axis)
 	{
@@ -91,6 +92,11 @@ float calcAngle(const uint8_t axis, const float x_axis, const float y_axis, cons
 	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                     *
 * INFORMATION:                                                            *
@@ -117,8 +123,8 @@ float constrainf(float amt, int low, int high)
 
 
 float oldSensorValue[2] = {0};
-float oldSensorValueRoll[50] = {0};
-float oldSensorValuePitch[50] = {0};
+float oldSensorValueRoll[12] = {0};
+float oldSensorValuePitch[12] = {0};
 
 int i = 0;
 
@@ -146,7 +152,6 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
 		mpu6000_read_accel(&accelProfile);	/*Reads data from accelerometer*/
 
 
-
 		float alpha = 0.5;
 		/*May need Low pass filter since the accelerometer may drift*/
 
@@ -164,7 +169,7 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
 		float RollValue = 0;
 		float PitchValue = 0;
 
-		for (int ii = 0; ii < 50; ii++)
+		for (int ii = 0; ii < 12; ii++)
 		{
 			RollValue = RollValue + oldSensorValueRoll[ii];
 			PitchValue = PitchValue + oldSensorValuePitch[ii];
@@ -172,13 +177,13 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
 		}
 
 
-		i = (i < 49)? i + 1:0;
+		i = (i < 11)? i + 1:0;
 
-		sensorValues[ROLL] = RollValue/50;
-		sensorValues[PITCH]  = PitchValue/50;
+		sensorValues[ROLL] = RollValue/12;
+		sensorValues[PITCH]  = PitchValue/12;
 
-		sensorValues[ROLL] = alpha*RollValue/50 + (1-alpha)*oldSensorValue[0];
-		sensorValues[PITCH] = alpha*PitchValue/50 + (1-alpha)*oldSensorValue[1];
+		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];
@@ -291,7 +296,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
 
 
 	/* -----calculate I component ---- */
-	float ITerm = pidProfileBuff->lastITerm[axis] + ITERM_SCALE * rateError * pidProfileBuff->dT * pidProfile->I[axis];
+	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
@@ -341,6 +346,11 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
 
 
 	/*----PID OUT----*/
+	if(!flags_IsSet_ID(systemFlags_armed_id) || (rc_input.Throttle < mixerConfig.minCheck))
+	{
+		ITerm = 0;
+		pidProfileBuff->lastITerm[axis] = 0;
+	}
 	pidProfile->PID_Out[axis] = constrainf(PTerm + ITerm + DTerm, -pidProfile->pid_out_limit, pidProfile->pid_out_limit);
 }
 
diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c
index c4af970..2c9dfaf 100644
--- a/UAV-ControlSystem/src/main.c
+++ b/UAV-ControlSystem/src/main.c
@@ -47,6 +47,10 @@ void init_system()
 	//Configure the clock
 	system_clock_config();
 
+	//init motors to run with oneshot 125, small delay
+	HAL_Delay(1000);
+	pwmEnableAllMotors(Oneshot125);
+
 	//Initializes all the pids that are used in the system. This part will also init the gyro and accelerometer.
 	pidInit();
 
@@ -97,9 +101,7 @@ void init_system()
 
 #endif
 
-	//init motors to run with oneshot 125, small delay
-	HAL_Delay(1000);
-	pwmEnableAllMotors(Oneshot125);
+
 
 }