diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c
index 5c04d52ee4..1eca555380 100644
--- a/src/main/flight/altitudehold.c
+++ b/src/main/flight/altitudehold.c
@@ -63,12 +63,11 @@ static int16_t initialThrottleHold;
 static void multirotorAltHold(void)
 {
     static uint8_t isAltHoldChanged = 0;
-    static int16_t AltHoldCorr = 0;
     // multirotor alt hold
     if (currentProfile.alt_hold_fast_change) {
         // rapid alt changes
         if (abs(rcCommand[THROTTLE] - initialThrottleHold) > currentProfile.alt_hold_deadband) {
-            errorAltitudeI = 0;
+            errorVelocityI = 0;
             isAltHoldChanged = 1;
             rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -currentProfile.alt_hold_deadband : currentProfile.alt_hold_deadband;
         } else {
@@ -76,22 +75,21 @@ static void multirotorAltHold(void)
                 AltHold = EstAlt;
                 isAltHoldChanged = 0;
             }
-            rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, masterConfig.escAndServoConfig.minthrottle + 100, masterConfig.escAndServoConfig.maxthrottle);
+            rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, masterConfig.escAndServoConfig.minthrottle, masterConfig.escAndServoConfig.maxthrottle);
         }
     } else {
         // slow alt changes for apfags
         if (abs(rcCommand[THROTTLE] - initialThrottleHold) > currentProfile.alt_hold_deadband) {
-            // Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms)
-            AltHoldCorr += rcCommand[THROTTLE] - initialThrottleHold;
-            AltHold += AltHoldCorr / 2000;
-            AltHoldCorr %= 2000;
+            // set velocity proportional to stick movement +100 throttle gives ~ +50 cm/s
+            setVelocity = (rcCommand[THROTTLE] - initialThrottleHold) / 2;
+            velocityControl = 1;
             isAltHoldChanged = 1;
         } else if (isAltHoldChanged) {
             AltHold = EstAlt;
-            AltHoldCorr = 0;
+            velocityControl = 0;
             isAltHoldChanged = 0;
         }
-        rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, masterConfig.escAndServoConfig.minthrottle + 100, masterConfig.escAndServoConfig.maxthrottle);
+        rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, masterConfig.escAndServoConfig.minthrottle, masterConfig.escAndServoConfig.maxthrottle);
     }
 }
 
@@ -121,7 +119,7 @@ void updateAltHoldState(void)
             f.BARO_MODE = 1;
             AltHold = EstAlt;
             initialThrottleHold = rcCommand[THROTTLE];
-            errorAltitudeI = 0;
+            errorVelocityI = 0;
             BaroPID = 0;
         }
     } else {
diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index b0a660bd55..31e7ca9f3e 100755
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -56,7 +56,9 @@ int16_t smallAngle = 0;
 
 int32_t EstAlt;                // in cm
 int32_t AltHold;
-int32_t errorAltitudeI = 0;
+int32_t setVelocity = 0;
+uint8_t velocityControl = 0;
+int32_t errorVelocityI = 0;
 
 int32_t vario = 0;                      // variometer in cm/s
 
@@ -394,10 +396,13 @@ int32_t calculateBaroPid(int32_t vel_tmp, float accZ_tmp, float accZ_old)
 
     // Altitude P-Controller
 
-    error = constrain(AltHold - EstAlt, -500, 500);
-    error = applyDeadband(error, 10);       // remove small P parametr to reduce noise near zero position
-    setVel = constrain((pidProfile->P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s
-
+    if (!velocityControl) {
+        error = constrain(AltHold - EstAlt, -500, 500);
+        error = applyDeadband(error, 10); // remove small P parameter to reduce noise near zero position
+        setVel = constrain((pidProfile->P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s
+    } else {
+        setVel = setVelocity;
+    }
     // Velocity PID-Controller
 
     // P
@@ -405,9 +410,9 @@ int32_t calculateBaroPid(int32_t vel_tmp, float accZ_tmp, float accZ_old)
     newBaroPID = constrain((pidProfile->P8[PIDVEL] * error / 32), -300, +300);
 
     // I
-    errorAltitudeI += (pidProfile->I8[PIDVEL] * error);
-    errorAltitudeI = constrain(errorAltitudeI, -(8192 * 200), (8192 * 200));
-    newBaroPID += errorAltitudeI / 8192;     // I in range +/-200
+    errorVelocityI += (pidProfile->I8[PIDVEL] * error);
+    errorVelocityI = constrain(errorVelocityI, -(8192 * 200), (8192 * 200));
+    newBaroPID += errorVelocityI / 8192;     // I in range +/-200
 
     // D
     newBaroPID -= constrain(pidProfile->D8[PIDVEL] * (accZ_tmp + accZ_old) / 512, -150, 150);
diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h
index 48a9058083..a1091fae8e 100644
--- a/src/main/flight/imu.h
+++ b/src/main/flight/imu.h
@@ -17,7 +17,9 @@
 
 #pragma once
 
-extern int32_t errorAltitudeI;
+extern int32_t errorVelocityI;
+extern uint8_t velocityControl;
+extern int32_t setVelocity;
 extern int32_t BaroPID;
 extern int16_t throttleAngleCorrection;