diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c
index 8c644f1..59ad95c 100644
--- a/UAV-ControlSystem/src/Flight/pid.c
+++ b/UAV-ControlSystem/src/Flight/pid.c
@@ -26,7 +26,7 @@
 #define PTERM_SCALE 0.032029f										/*P-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 BAROMETER_SCALE 20
+#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)*/
@@ -79,12 +79,19 @@ float convertData(int inputRange, int outputRange, int offset, float value)
 	return ((float)outputRange/(float)inputRange)*(value-(float)offset);
 }
 
+uint8_t FlagVelocityLimit = 0;
+float VelocityCompensation = 0;
+
 /**************************************************************************
 * BRIEF: Update current sensor values				                      *
 * INFORMATION:                       									  *
 **************************************************************************/
 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)
 	{
 	case PID_ID_GYRO:
@@ -117,7 +124,36 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
 		break;
 	case PID_ID_BAROMETER:
 
-		sensorValues[0] =  barometer_GetCurrentAveragedtAltitude()*BAROMETER_SCALE;
+		sensorValues[0] =  barometer_GetCurrentAveragedtAltitude();
+
+		counter += 1;
+
+		if (counter > 2)
+		{
+
+			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];
+		sensorValues[0]*=BAROMETER_SCALE;
+
 		break;
 	default:
 
@@ -353,7 +389,8 @@ void pidRun(uint8_t ID)
 		else
 		{
 			pidUAV(&PidProfile[PID_ID_BAROMETER], &PidProfileBuff[PID_ID_BAROMETER]);
-			PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainf(PidProfile[PID_ID_BAROMETER].PID_Out[0], -15, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit);
+
+			PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainf(PidProfile[PID_ID_BAROMETER].PID_Out[0], -15, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit + (int)VelocityCompensation);
 
 		}