diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c
index 59ad95c..b9dd38c 100644
--- a/UAV-ControlSystem/src/Flight/pid.c
+++ b/UAV-ControlSystem/src/Flight/pid.c
@@ -29,7 +29,7 @@
 #define BAROMETER_SCALE 11
 
 #define RADIO_RANGE 500												/*Radio range input*/
-#define BAROMETER_RANGE 3											/*Determines the range of the maximum height (limits the rc input)*/
+#define BAROMETER_RANGE 10											/*Determines the range of the maximum height (limits the rc input)*/
 #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)*/
@@ -90,7 +90,6 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
 {
 	static float last_micros = 0;
 	static float oldHeightValue = 0;
-	static uint8_t counter = 0;
 
 	switch (ID_profile)
 	{
@@ -124,34 +123,16 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
 		break;
 	case PID_ID_BAROMETER:
 
-		sensorValues[0] =  barometer_GetCurrentAveragedtAltitude();
+		float current_micros = clock_get_us()/1000000;
+		float delta_t = (current_micros - last_micros);
+		float current_height = barometer_GetCurrentAveragedtAltitude();
 
-		counter += 1;
+		last_micros = current_micros;
 
-		if (counter > 2)
-		{
+		sensorValues[0] = ((current_height - oldHeightValue)/delta_t);
+		sensorValues[0] = (sensorValues[0] < - 6 || sensorValues[0] >  6)? sensorValues[0]:0;
 
-			float current_micros = clock_get_us()/1000000;
-			float delta_t = (current_micros - last_micros);
-			last_micros = current_micros;
-			counter = 0;
-
-			float current_velocity = (sensorValues[0] - oldHeightValue)/delta_t;
-
-			if( current_velocity<= -10)
-			{
-
-				VelocityCompensation = ABS_FLOAT(current_velocity*3);
-
-			}
-			else
-			{
-				VelocityCompensation = 0;
-			}
-
-		}
-
-		oldHeightValue = sensorValues[0];
+		oldHeightValue = current_height;
 		sensorValues[0]*=BAROMETER_SCALE;
 
 		break;
@@ -212,9 +193,9 @@ void getPointRate(float *desiredCommand, uint8_t ID_profile)
 		break;
 	case PID_ID_BAROMETER:
 
-		//desiredCommand[THROTTLE] = convertData(RADIO_RANGE, BAROMETER_RANGE, 0, rc_input.Throttle*throttleRate);
-
-		desiredCommand[THROTTLE] = DESIRED_HEIGHT*BAROMETER_SCALE;
+		float currentThrottle = rc_input.Throttle - 1500;
+		float velocity = (currentThrottle < - 10 || currentThrottle > 10 )? currentThrottle:0;
+		desiredCommand[THROTTLE] = convertData(RADIO_RANGE, BAROMETER_RANGE, 0, rc_input.Throttle)*BAROMETER_SCALE;
 
 		break;
 	default: