diff --git a/UAV-ControlSystem/inc/Flight/pid.h b/UAV-ControlSystem/inc/Flight/pid.h
index dd9f80f..a19bd81 100644
--- a/UAV-ControlSystem/inc/Flight/pid.h
+++ b/UAV-ControlSystem/inc/Flight/pid.h
@@ -75,4 +75,6 @@ void pidInit();
 **************************************************************************/
 void pidRun(uint8_t ID);
 
+void pidEproom(void);
+
 #endif /* FLIGHT_PID_H_ */
diff --git a/UAV-ControlSystem/inc/drivers/accel_gyro.h b/UAV-ControlSystem/inc/drivers/accel_gyro.h
index e8cd064..fd42421 100644
--- a/UAV-ControlSystem/inc/drivers/accel_gyro.h
+++ b/UAV-ControlSystem/inc/drivers/accel_gyro.h
@@ -101,7 +101,7 @@
 #define MPU6000_REV_D9				0x59
 #define MPU6000_REV_D10				0x5A
 
-#define YAW_ROT_0
+#define YAW_ROT_180
 
 typedef struct gyro_t {
 	int16_t gyroX, gyroY, gyroZ;       /* Gyroscope data converted into °/s */
diff --git a/UAV-ControlSystem/inc/utilities.h b/UAV-ControlSystem/inc/utilities.h
index 26fdf58..446ddea 100644
--- a/UAV-ControlSystem/inc/utilities.h
+++ b/UAV-ControlSystem/inc/utilities.h
@@ -83,6 +83,6 @@ void Error_Handler(void);
 
 uint8_t reverse(uint8_t byte);
 
-uint16_t constrain(uint16_t value, uint16_t min, uint16_t max);
+int16_t constrain(int16_t value, int16_t min, int16_t max);
 
 #endif /* UTILITIES_H_ */
diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c
index da126f9..b614907 100644
--- a/UAV-ControlSystem/src/Flight/pid.c
+++ b/UAV-ControlSystem/src/Flight/pid.c
@@ -18,6 +18,7 @@
 #include "drivers/sbus.h"
 #include "scheduler/scheduler.h"
 #include <math.h>
+#include "drivers/failsafe_toggles.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*/
@@ -25,7 +26,7 @@
 
 #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 90										/*Determines the range of the maximum angle  (limits the rc input) & (Accelerometer takes int to max 16 G)*/
+#define ACCELEROMETER_RANGE 35										/*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)*/
 
@@ -55,6 +56,38 @@ pidProfileBuff_t PidProfileBuff[PID_COUNT] = {0};
 accel_t accelProfile;												/*Struct profile for input data from sensor*/
 gyro_t gyroProfile;													/*Struct profile for input data from sensor*/
 
+pt1Filter_t accelFilter[2] = {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;
+	float angle_offset = (z_axis < 0 )? 90: 0;
+
+	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;
+}
 
 /**************************************************************************
 * BRIEF: Scales data from input range to output range                     *
@@ -62,7 +95,8 @@ gyro_t gyroProfile;													/*Struct profile for input data from sensor*/
 **************************************************************************/
 float convertData(int inputRange, int outputRange, int offset, float value)
 {
-	return (outputRange/inputRange)*(value-offset);
+	return ((float)outputRange/(float)inputRange)*(value-(float)offset);
+	//return 1.0;
 }
 
 /**************************************************************************
@@ -79,12 +113,21 @@ float constrainf(float amt, int low, int high)
 		return amt;
 }
 
+
+float oldSensorValue[2] = {0};
+float oldSensorValueRoll[50] = {0};
+float oldSensorValuePitch[50] = {0};
+
+int i = 0;
+
 /**************************************************************************
 * BRIEF: Update current sensor values				                      *
 * INFORMATION:                       									  *
 **************************************************************************/
-void getCurrentValues(float *sensorValues, uint8_t ID_profile)
+void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
 {
+
+
 	switch (ID_profile)
 	{
 	case PID_ID_GYRO:
@@ -100,9 +143,47 @@ void getCurrentValues(float *sensorValues, 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*/
-		sensorValues[ROLL] = atan2(accelProfile.accelXconv,accelProfile.accelZconv)*180*M_PI;
-		sensorValues[PITCH] = atan2(-accelProfile.accelXconv, sqrt(accelProfile.accelYconv*accelProfile.accelYconv + accelProfile.accelZconv*accelProfile.accelZconv))*180/M_PI;
+
+		float X_roll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
+		float X_pitch = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
+
+		oldSensorValueRoll[i] = X_roll;
+		oldSensorValuePitch[i] = X_pitch;
+
+		float RollValue = 0;
+		float PitchValue = 0;
+
+		for (int ii = 0; ii < 50; ii++)
+		{
+			RollValue = RollValue + oldSensorValueRoll[ii];
+			PitchValue = PitchValue + oldSensorValuePitch[ii];
+
+		}
+
+
+		i = (i < 49)? i + 1:0;
+
+		sensorValues[ROLL] = RollValue/50;
+		sensorValues[PITCH]  = PitchValue/50;
+
+		sensorValues[ROLL] = alpha*RollValue/50 + (1-alpha)*oldSensorValue[0];
+		sensorValues[PITCH] = alpha*PitchValue/50 + (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);
 
 		break;
 	case PID_ID_COMPASS:
@@ -129,7 +210,7 @@ void getPointRate(float *desiredCommand, uint8_t ID_profile)
 	{
 	case PID_ID_GYRO:
 
-		if (!PidProfile[PID_ID_ACCELEROMETER].pidEnabled)
+		if (!(PidProfile[PID_ID_ACCELEROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_acceleromter_id)))
 		{
 			desiredCommand[ROLL] = convertData(RADIO_RANGE, GYRO_RANGE, 0, PidProfile[PID_ID_ACCELEROMETER].PID_Out[ROLL]);
 			desiredCommand[PITCH] = convertData(RADIO_RANGE, GYRO_RANGE, 0, PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH]);
@@ -184,7 +265,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
 
 
 	/* -----calculate P component ---- */
-	float PTerm = PTERM_SCALE * rateError * pidProfile->P[axis] * pidProfile-> PIDweight[axis] / 100;
+	float PTerm = PTERM_SCALE * rateError * (float)pidProfile->P[axis] * (float)pidProfile-> PIDweight[axis] / 100.0;
 
 	// Constrain YAW by yaw_p_limit value
 	if (axis == YAW)
@@ -247,7 +328,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
 			delta = pt1FilterApply4(&pidProfileBuff->deltaFilter[axis], delta, pidProfile->dterm_lpf, pidProfileBuff->dT);
 		}
 
-		DTerm = DTERM_SCALE * delta * pidProfile->D[axis] * pidProfile->PIDweight[axis] / 100;
+		DTerm = DTERM_SCALE * delta * (float)pidProfile->D[axis] * (float)pidProfile->PIDweight[axis] / 100.0;
 		DTerm = constrainf(DTerm, -PID_MAX_D, PID_MAX_D);
 	}
 
@@ -301,7 +382,7 @@ void pidRun(uint8_t ID)
 		break;
 	case PID_ID_ACCELEROMETER:
 
-		if (!PidProfile[PID_ID_ACCELEROMETER].pidEnabled)
+		if (!(PidProfile[PID_ID_ACCELEROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_acceleromter_id)))
 		{
 			PidProfile[PID_ID_ACCELEROMETER].PID_Out[ROLL] = rc_input.Roll;
 			PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH] = rc_input.Pitch;
@@ -341,6 +422,8 @@ void pidRun(uint8_t ID)
 	}
 }
 
+/*--------------------------------------------Init Functions----------------------------------------------------------------------------------*/
+
 /**************************************************************************
 * BRIEF: Initializes a certain pidbuffer profile connected to             *
 * 		 a PID controller 												  *
@@ -415,14 +498,19 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
 	PidProfile[ID].PID_Out[PITCH] = 0;
 	PidProfile[ID].PID_Out[YAW] = 0;
 
-	PidProfile[ID].PIDweight[ROLL] = 100;
-	PidProfile[ID].PIDweight[PITCH] = 100;
-	PidProfile[ID].PIDweight[YAW] = 100;
+
+	PidProfile[ID].P[ROLL] = 10;
+	PidProfile[ID].P[PITCH] = 10;
+	PidProfile[ID].P[YAW] = 10;
 
 	switch (ID)
 	{
 	case PID_ID_GYRO:
 
+		PidProfile[ID].PIDweight[ROLL] = 100;
+		PidProfile[ID].PIDweight[PITCH] = 100;
+		PidProfile[ID].PIDweight[YAW] = 100;
+
 		PidProfile[ID].pidEnabled = true;
 		PidProfile[ID].dterm_lpf = 90;
 		PidProfile[ID].pid_out_limit = 3000;
@@ -430,18 +518,30 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
 		break;
 	case PID_ID_ACCELEROMETER:
 
-		PidProfile[ID].pidEnabled = false;
-		//PidProfile[ID].dterm_lpf = 90;
-		//PidProfile[ID].pid_out_limit = 3000;
+		PidProfile[ID].PIDweight[ROLL] = 2;
+		PidProfile[ID].PIDweight[PITCH] = 2;
+		PidProfile[ID].PIDweight[YAW] = 100;
+
+		PidProfile[ID].pidEnabled = true;
+		PidProfile[ID].dterm_lpf = 90;
+		PidProfile[ID].pid_out_limit = 1000;
 
 		break;
 	case PID_ID_COMPASS:
 
+		PidProfile[ID].PIDweight[ROLL] = 100;
+		PidProfile[ID].PIDweight[PITCH] = 100;
+		PidProfile[ID].PIDweight[YAW] = 100;
+
 		PidProfile[ID].pidEnabled = false;
 
 		break;
 	case PID_ID_BAROMETER:
 
+		PidProfile[ID].PIDweight[ROLL] = 100;
+		PidProfile[ID].PIDweight[PITCH] = 100;
+		PidProfile[ID].PIDweight[YAW] = 100;
+
 		PidProfile[ID].pidEnabled = false;
 
 		break;
@@ -471,3 +571,12 @@ void pidInit()
 	pidUAVInit(&PidProfile[PID_ID_BAROMETER], PID_ID_BAROMETER);
 	pidUAVInit(&PidProfile[PID_ID_COMPASS], PID_ID_COMPASS);
 }
+
+void pidEproom(void)
+{
+
+	PidProfile[PID_ID_ACCELEROMETER].PIDweight[ROLL] = 2;
+	PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 2;
+	PidProfile[PID_ID_ACCELEROMETER].PIDweight[YAW] = 100;
+
+}
diff --git a/UAV-ControlSystem/src/config/cli.c b/UAV-ControlSystem/src/config/cli.c
index f53a4e8..e6fda2c 100644
--- a/UAV-ControlSystem/src/config/cli.c
+++ b/UAV-ControlSystem/src/config/cli.c
@@ -278,6 +278,10 @@ typedef enum
 	COMMAND_ID_PID_ACCEL_YAW_P_LIMIT,
 	COMMAND_ID_PID_ACCEL_OUT_LIMIT,
 
+	/* Enable the different pid loops */
+	COMMAND_ID_PID_GYRO_ISENABLED,
+	COMMAND_ID_PID_ACCEL_ISENABLED,
+
 	/* Counter for the amount of commands */
 	COMMAND_ID_COUNT,
 
@@ -544,110 +548,122 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = {
 		//GYRO P
 		[COMMAND_ID_PID_GYRO_P_ROLL] =
 		{
-				"pid_gyro_p_roll", 	COMMAND_ID_PID_GYRO_P_ROLL, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 5, VAL_UINT_8, .valueRange = {0, 100}
+				"pid_gyro_p_roll", 	COMMAND_ID_PID_GYRO_P_ROLL, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 5, VAL_UINT_8, .valueRange = {0, 255}
 		},
 		[COMMAND_ID_PID_GYRO_P_PITCH] =
 		{
-				"pid_gyro_p_pitch", 	COMMAND_ID_PID_GYRO_P_PITCH, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 6, VAL_UINT_8, .valueRange = {0, 100}
+				"pid_gyro_p_pitch", 	COMMAND_ID_PID_GYRO_P_PITCH, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 6, VAL_UINT_8, .valueRange = {0, 255}
 		},
 		[COMMAND_ID_PID_GYRO_P_YAW] =
 		{
-				"pid_gyro_p_yaw", 	COMMAND_ID_PID_GYRO_P_YAW, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 7, VAL_UINT_8, .valueRange = {0, 100}
+				"pid_gyro_p_yaw", 	COMMAND_ID_PID_GYRO_P_YAW, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 7, VAL_UINT_8, .valueRange = {0, 255}
 		},
 
 		//GYRO I
 		[COMMAND_ID_PID_GYRO_I_ROLL] =
 		{
-				"pid_gyro_i_roll", 	COMMAND_ID_PID_GYRO_I_ROLL, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 8, VAL_UINT_8, .valueRange = {0, 100}
+				"pid_gyro_i_roll", 	COMMAND_ID_PID_GYRO_I_ROLL, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 8, VAL_UINT_8, .valueRange = {0, 255}
 		},
 		[COMMAND_ID_PID_GYRO_I_PITCH] =
 		{
-				"pid_gyro_i_pitch", 	COMMAND_ID_PID_GYRO_I_PITCH, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 9, VAL_UINT_8, .valueRange = {0, 100}
+				"pid_gyro_i_pitch", 	COMMAND_ID_PID_GYRO_I_PITCH, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 9, VAL_UINT_8, .valueRange = {0, 255}
 		},
 		[COMMAND_ID_PID_GYRO_I_YAW] =
 		{
-				"pid_gyro_i_yaw", 	COMMAND_ID_PID_GYRO_I_YAW, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 10, VAL_UINT_8, .valueRange = {0, 100}
+				"pid_gyro_i_yaw", 	COMMAND_ID_PID_GYRO_I_YAW, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 10, VAL_UINT_8, .valueRange = {0, 255}
 		},
 
 		//GYRO D
 		[COMMAND_ID_PID_GYRO_D_ROLL] =
 		{
-				"pid_gyro_d_roll", 	COMMAND_ID_PID_GYRO_D_ROLL, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 11, VAL_UINT_8, .valueRange = {0, 100}
+				"pid_gyro_d_roll", 	COMMAND_ID_PID_GYRO_D_ROLL, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 11, VAL_UINT_8, .valueRange = {0, 255}
 		},
 		[COMMAND_ID_PID_GYRO_D_PITCH] =
 		{
-				"pid_gyro_d_pitch", 	COMMAND_ID_PID_GYRO_D_PITCH, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 12, VAL_UINT_8, .valueRange = {0, 100}
+				"pid_gyro_d_pitch", 	COMMAND_ID_PID_GYRO_D_PITCH, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 12, VAL_UINT_8, .valueRange = {0, 255}
 		},
 		[COMMAND_ID_PID_GYRO_D_YAW] =
 		{
-				"pid_gyro_d_yaw", 	COMMAND_ID_PID_GYRO_D_YAW, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 13, VAL_UINT_8, .valueRange = {0, 100}
+				"pid_gyro_d_yaw", 	COMMAND_ID_PID_GYRO_D_YAW, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 13, VAL_UINT_8, .valueRange = {0, 255}
 		},
 
 		//Gyro filter and limits
 		[COMMAND_ID_PID_GYRO_DTERM_LPF] =
 		{
-				"pid_gyro_dterm_lpf", 	COMMAND_ID_PID_GYRO_DTERM_LPF, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 14, VAL_UINT_16, .valueRange = {0, 100}
+				"pid_gyro_dterm_lpf", 	COMMAND_ID_PID_GYRO_DTERM_LPF, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 14, VAL_UINT_16, .valueRange = {0, 255}
 		},
 		[COMMAND_ID_PID_GYRO_PTERM_YAW_LPF] =
 		{
-				"pid_gyro_pterm_yaw_lpf", 	COMMAND_ID_PID_GYRO_PTERM_YAW_LPF, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 16, VAL_UINT_16, .valueRange = {0, 100}
+				"pid_gyro_pterm_yaw_lpf", 	COMMAND_ID_PID_GYRO_PTERM_YAW_LPF, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 16, VAL_UINT_16, .valueRange = {0, 255}
 		},
 		[COMMAND_ID_PID_GYRO_YAW_P_LIMIT] =
 		{
-				"pid_gyro_yaw_lpf", 	COMMAND_ID_PID_GYRO_YAW_P_LIMIT, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 18, VAL_UINT_16, .valueRange = {0, 100}
+				"pid_gyro_yaw_lpf", 	COMMAND_ID_PID_GYRO_YAW_P_LIMIT, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 18, VAL_UINT_16, .valueRange = {0, 255}
 		},
 		[COMMAND_ID_PID_GYRO_OUT_LIMIT] =
 		{
-				"pid_gyro_out_limit", 	COMMAND_ID_PID_GYRO_OUT_LIMIT, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 20, VAL_UINT_16, .valueRange = {0, 100}
+				"pid_gyro_out_limit", 	COMMAND_ID_PID_GYRO_OUT_LIMIT, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 20, VAL_UINT_16, .valueRange = {0, 255}
 		},
 
 		//ACCEL P
 		[COMMAND_ID_PID_ACCEL_P_ROLL] =
 		{
-				"pid_accel_p_roll", 	COMMAND_ID_PID_ACCEL_P_ROLL, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 5, VAL_UINT_8, .valueRange = {0, 100}
+				"pid_accel_p_roll", 	COMMAND_ID_PID_ACCEL_P_ROLL, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 5, VAL_UINT_8, .valueRange = {0, 255}
 		},
 		[COMMAND_ID_PID_ACCEL_P_PITCH] =
 		{
-				"pid_accel_p_pitch", 	COMMAND_ID_PID_ACCEL_P_PITCH, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 6, VAL_UINT_8, .valueRange = {0, 100}
+				"pid_accel_p_pitch", 	COMMAND_ID_PID_ACCEL_P_PITCH, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 6, VAL_UINT_8, .valueRange = {0, 255}
 		},
 
 		//ACCEL I
 		[COMMAND_ID_PID_ACCEL_I_ROLL] =
 		{
-				"pid_accel_i_roll", 	COMMAND_ID_PID_ACCEL_I_ROLL, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 8, VAL_UINT_8, .valueRange = {0, 100}
+				"pid_accel_i_roll", 	COMMAND_ID_PID_ACCEL_I_ROLL, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 8, VAL_UINT_8, .valueRange = {0, 255}
 		},
 		[COMMAND_ID_PID_ACCEL_I_PITCH] =
 		{
-				"pid_accel_i_pitch", 	COMMAND_ID_PID_ACCEL_I_PITCH, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 9, VAL_UINT_8, .valueRange = {0, 100}
+				"pid_accel_i_pitch", 	COMMAND_ID_PID_ACCEL_I_PITCH, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 9, VAL_UINT_8, .valueRange = {0, 255}
 		},
 
 		//ACCEL D
 		[COMMAND_ID_PID_ACCEL_D_ROLL] =
 		{
-				"pid_accel_d_roll", 	COMMAND_ID_PID_ACCEL_D_ROLL, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 11, VAL_UINT_8, .valueRange = {0, 100}
+				"pid_accel_d_roll", 	COMMAND_ID_PID_ACCEL_D_ROLL, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 11, VAL_UINT_8, .valueRange = {0, 255}
 		},
 		[COMMAND_ID_PID_ACCEL_D_PITCH] =
 		{
-				"pid_accel_d_pitch", 	COMMAND_ID_PID_ACCEL_D_PITCH, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 12, VAL_UINT_8, .valueRange = {0, 100}
+				"pid_accel_d_pitch", 	COMMAND_ID_PID_ACCEL_D_PITCH, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 12, VAL_UINT_8, .valueRange = {0, 255}
 		},
 
 		//ACCEL Filters and limits
 		[COMMAND_ID_PID_ACCEL_DTERM_LPF] =
 		{
-				"pid_accel_dterm_lpf", 	COMMAND_ID_PID_ACCEL_DTERM_LPF, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 14, VAL_UINT_16, .valueRange = {0, 100}
+				"pid_accel_dterm_lpf", 	COMMAND_ID_PID_ACCEL_DTERM_LPF, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 14, VAL_UINT_16, .valueRange = {0, 65000}
 		},
 		[COMMAND_ID_PID_ACCEL_PTERM_YAW_LPF] =
 		{
-				"pid_accel_pterm_yaw_lpf", 	COMMAND_ID_PID_ACCEL_PTERM_YAW_LPF, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 15, VAL_UINT_16, .valueRange = {0, 100}
+				"pid_accel_pterm_yaw_lpf", 	COMMAND_ID_PID_ACCEL_PTERM_YAW_LPF, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 16, VAL_UINT_16, .valueRange = {0, 65000}
 		},
 		[COMMAND_ID_PID_ACCEL_YAW_P_LIMIT] =
 		{
-				"pid_accel_yaw_p_limit", 	COMMAND_ID_PID_ACCEL_YAW_P_LIMIT, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 17, VAL_UINT_16, .valueRange = {0, 100}
+				"pid_accel_yaw_p_limit", 	COMMAND_ID_PID_ACCEL_YAW_P_LIMIT, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 18, VAL_UINT_16, .valueRange = {0, 65000}
 		},
 		[COMMAND_ID_PID_ACCEL_OUT_LIMIT] =
 		{
-				"pid_accel_yaw_p_limit", 	COMMAND_ID_PID_ACCEL_OUT_LIMIT, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 18, VAL_UINT_16, .valueRange = {0, 100}
+				"pid_accel_out_limit", 	COMMAND_ID_PID_ACCEL_OUT_LIMIT, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 20, VAL_UINT_16, .valueRange = {0, 65000}
 		},
+
+		/* Enable pid loops */
+		[COMMAND_ID_PID_GYRO_ISENABLED] =
+		{
+				"pid_gyro_isenabled", 	COMMAND_ID_PID_GYRO_ISENABLED, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_BOOL, .valueRange = {0, 1}
+		},
+		[COMMAND_ID_PID_ACCEL_ISENABLED] =
+		{
+				"pid_accel_isenabled", 	COMMAND_ID_PID_ACCEL_ISENABLED, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_BOOL, .valueRange = {0, 1}
+		},
+
+
 };
 
 /***********************************************************************
diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c
index a40e668..c77a4f7 100644
--- a/UAV-ControlSystem/src/drivers/accel_gyro.c
+++ b/UAV-ControlSystem/src/drivers/accel_gyro.c
@@ -241,6 +241,9 @@ bool mpu6000_init(gyro_t* gyro, accel_t* accel)
 	mpu6000_read_gyro_offset(gyro);
 	mpu6000_read_acc_offset(accel);
 
+	HAL_Delay(60);
+
+
 	return true;
 }
 
diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c
index 4179c9a..3da3fec 100644
--- a/UAV-ControlSystem/src/drivers/motormix.c
+++ b/UAV-ControlSystem/src/drivers/motormix.c
@@ -47,7 +47,7 @@ uint16_t motor_output[MOTOR_COUNT];
 mixerConfig_s mixerConfig = {
 	.minThrottle = 1050,
 	.maxThrottle = MAX_PULSE - 100,
-	.minCommand = 1000,
+	.minCommand = 990,
 	.maxCommand = MAX_PULSE,
 	.minCheck = 1010,
 	.pid_at_min_throttle = true,
@@ -114,8 +114,8 @@ void mix()
 			 * Calculation is: Output from control system * weight from model for each motor
 			 */
 			RPY_Mix[i] = \
-					PidProfile[PID_ID_GYRO].PID_Out[ROLL] 	*	mixerUAV[i].roll 	+ 	\
-					PidProfile[PID_ID_GYRO].PID_Out[PITCH] 	* 	mixerUAV[i].pitch	+ 	\
+					(int)PidProfile[PID_ID_GYRO].PID_Out[ROLL] 	*	mixerUAV[i].roll 	+ 	\
+					(int)PidProfile[PID_ID_GYRO].PID_Out[PITCH] 	* 	mixerUAV[i].pitch	+ 	\
 					PidProfile[PID_ID_GYRO].PID_Out[YAW]	*	mixerUAV[i].yaw		*	((mixerConfig.yaw_reverse_direction) ? -1 : 1);
 
 
@@ -194,15 +194,21 @@ void mix()
 		{
 			RPY_Mix[i] = \
 					throttle 								*	mixerUAV[i].throttle	+	\
-					PidProfile[PID_ID_GYRO].PID_Out[ROLL] 	*	mixerUAV[i].roll 	+ 	\
-					PidProfile[PID_ID_GYRO].PID_Out[PITCH] 	* 	mixerUAV[i].pitch	+ 	\
+					(int)PidProfile[PID_ID_GYRO].PID_Out[ROLL] 	*	mixerUAV[i].roll 	+ 	\
+					(int)PidProfile[PID_ID_GYRO].PID_Out[PITCH] 	* 	mixerUAV[i].pitch	+ 	\
 					PidProfile[PID_ID_GYRO].PID_Out[YAW]	*	mixerUAV[i].yaw		*	((mixerConfig.yaw_reverse_direction) ? -1 : 1);
 		}
 
 
+
+
 		/* Mixer Low Scale - Scaling output around low throttle */
 		if (flags_IsSet_ID(systemFlags_mixerlowscale_id))
 		{
+			for (int i = 0; i < MOTOR_COUNT; i++)
+				// Find the minimum and maximum motor output
+				if (RPY_Mix[i] < RPY_Mix_Min) RPY_Mix_Min = RPY_Mix[i];
+
 			uint16_t RPY_Mix_Min_Overshoot = (RPY_Mix_Min < mixerConfig.minThrottle) ? mixerConfig.minThrottle - RPY_Mix_Min : 0;
 
 			if (RPY_Mix_Min_Overshoot > 0 ) //|| RPY_Mix_Max_Overshoot > 0)
@@ -217,8 +223,12 @@ void mix()
 				for (int i = 0; i < MOTOR_COUNT; i++)
 					RPY_Mix[i] = (((float)(RPY_Mix[i] - throttle)) * (1 - mix_scale_reduction)) + throttle;
 			}
+			/* Recalculating RPY_Mix_Min */
+			RPY_Mix_Min = throttleMid;
 		}
 
+
+
 		for (int i = 0; i < MOTOR_COUNT; i++)
 		{
 			// Find the minimum and maximum motor output
diff --git a/UAV-ControlSystem/src/drivers/motors.c b/UAV-ControlSystem/src/drivers/motors.c
index b2a83d9..53bb569 100644
--- a/UAV-ControlSystem/src/drivers/motors.c
+++ b/UAV-ControlSystem/src/drivers/motors.c
@@ -248,7 +248,7 @@ void calibrateMotors()
 		pwmActivateAllMotors();
 
 		//First set the motor output to the maximum allowed throttle
-		for (uint8_t i = 1; i < 11; i++ ) pwmAdjustSpeedOfMotor(i,MotorPWMPeriode);
+		for (uint8_t i = 1; i < 11; i++ ) pwmAdjustSpeedOfMotor(i,MAX_PULSE);
 
 		//wait for a little while
 		HAL_Delay(6200);
diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c
index 3c71f7e..3b7cdcd 100644
--- a/UAV-ControlSystem/src/main.c
+++ b/UAV-ControlSystem/src/main.c
@@ -53,6 +53,8 @@ void init_system()
 	/* read saved variables from eeprom, in most cases eeprom should be read after a lot of the initializes  */
 	readEEPROM();
 
+	pidEproom();
+
 	//initialize the CLI NOTE: Cant use the same usart as anything else or there will be some big trouble
 	cliInit(USART3);
 
diff --git a/UAV-ControlSystem/src/utilities.c b/UAV-ControlSystem/src/utilities.c
index 2e22185..7c2192c 100644
--- a/UAV-ControlSystem/src/utilities.c
+++ b/UAV-ControlSystem/src/utilities.c
@@ -219,7 +219,7 @@ uint8_t reverse(uint8_t byte)
 	return byte;
 }
 
-uint16_t constrain(uint16_t value, uint16_t min, uint16_t max)
+int16_t constrain(int16_t value, int16_t min, int16_t max)
 {
 	if (value < min)
 		return min;