diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c
index fbb1f94..366a9a8 100644
--- a/UAV-ControlSystem/src/Flight/pid.c
+++ b/UAV-ControlSystem/src/Flight/pid.c
@@ -374,6 +374,7 @@ void pidRun(uint8_t ID)
 			PidProfile[PID_ID_ACCELEROMETER].PID_Out[ROLL] = rc_input.Roll;
 			PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH] = rc_input.Pitch;
 			flagAccBuff = 0;
+			throttleRate = 1;
 		}
 		else
 		{
diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c
index 8bd75e8..2a40d42 100644
--- a/UAV-ControlSystem/src/drivers/accel_gyro.c
+++ b/UAV-ControlSystem/src/drivers/accel_gyro.c
@@ -527,6 +527,7 @@ bool mpu6000_who_am_i()
 /* 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^4) //  that means a CMP_FACTOR of 1024 (2^10)
+#define ACCEL_LPF_FACTOR	16
 
 #define GetMagnitude(x)		(x*x)
 #define Low_Magnitude		(GetMagnitude(0.85))
@@ -554,7 +555,7 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro)
 	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;
+		smooth[i] = lpf_Acc[i] / ACCEL_LPF_FACTOR;
 
 		//Save the sign(+/-) of the value
 		sign[i] = (accelConv[i]< 0) ? -1 : 1;
@@ -587,8 +588,13 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro)
 		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;
+// TODO: This is the original code. Test to set factors for them separatelly
+//		accel->rollAngle 	 += (a_RollAngle - accel->rollAngle) / GYRO_ACC_DIV_FACTOR;
+//		accel->pitchAngle	 += (a_PitchAngle - accel->pitchAngle) / GYRO_ACC_DIV_FACTOR;
+
+
+		accel->rollAngle 	 += (a_RollAngle/4 - accel->rollAngle/GYRO_ACC_DIV_FACTOR);
+		accel->pitchAngle	 += (a_PitchAngle/4 - accel->pitchAngle/GYRO_ACC_DIV_FACTOR);
 	}
 
 	//Always constran the angular values within the possible ranges 90 to -90
diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c
index e296f2d..25ba171 100644
--- a/UAV-ControlSystem/src/drivers/motormix.c
+++ b/UAV-ControlSystem/src/drivers/motormix.c
@@ -103,7 +103,7 @@ void mix()
 	int16_t RPY_Mix[MOTOR_COUNT];		// Roll Pitch and Yaw variables array
 	int16_t RPY_Mix_Min = 0; 			// Stores the minimum desired command for any motor
 	int16_t RPY_Mix_Max = 0;			// Maximum desired command for any motor
-	int16_t throttle = PidProfile[PID_ID_BAROMETER].PID_Out[THROTTLE]/**throttleRate*/;
+	int16_t throttle = PidProfile[PID_ID_BAROMETER].PID_Out[THROTTLE]*throttleRate;
 
 	if (PidProfile[PID_ID_BAROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_barometer_id)) throttle += HoverForce;
 
diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c
index 38610a8..09e657d 100644
--- a/UAV-ControlSystem/src/tasks_main.c
+++ b/UAV-ControlSystem/src/tasks_main.c
@@ -256,9 +256,9 @@ void systemTaskSonar(void)
 
 void systemTaskAltitude(void)
 {
-	uint8_t c[50];
-		sprintf(c, "Roll: %-6f, Pitch %-6f\r", accelProfile.rollAngle, accelProfile.pitchAngle);
-		usart_transmit(&cliUsart, &c, 50, 1000000000);
+//	uint8_t c[50];
+//		sprintf(c, "Roll: %-6f, Pitch %-6f\r", accelProfile.rollAngle, accelProfile.pitchAngle);
+//		usart_transmit(&cliUsart, &c, 50, 1000000000);
 
 	//Keep track of the vehicles current altitude, based on some sensor. In this case either barometer or sonar